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I  INTRODUCTION 


AB-  flieuifig 

The  Air  Force  has  a  need  to  aaintain  force  survivability  and  base 
operability  during  wartiae  scenarios  in  chemical,  biological  and 
radiological  environments.  The  Robotic  Telepresence  program  at  the 
Armstrong  Aerospace  ledical  Research  laboratory  (AAIRL)  at  Vright 
Patterson  Air  Force  Base,  Ohio,  is  based  on  the  need  to  project  human 
intelligence,  perceptual  capabilities,  and  motor  skills  into  hostile 
environments  through  the  use  of  human  driven  robotic  systems,  thereby 
removing  humans  from  the  hazardous  environment.  The  Robotic 
Telepresence  program  at  AAKRL  investigates  the  feasibility  of  utilizing 
remote  human- in- the- loop  control  of  mobile  dexterous  robots  to  perform 
tasks  such  as  aircraft  inspection  and  servicing,  explosive  ordinance 
disposal,  and  environmental  monitoring  and  decontamination. 

The  Robotic  Telepresence  concept  projects  human  judgment,  dexterity 
and  adaptability  in  real  time  into  a  lethal  environment.  The  program  at 
AAMRL  will  develop  a  series  of  dynamic  telepresence  test  cells 
incorporating  driving  systems  attached  to  the  human  arm  and  hand  as  well 
as  remote  driven  systems  involving  manipulators  and  dexterous 
end- effectors,  amongst  other  state-of-the-art  components.  The  remote 
system  currently  being  evaluated  consists  of  the  Utah/MIT  dexterous 
hands  as  suitable  end- effectors  to  be  attached  to  the  end  of  robot  arms 
such  as  the  Merlin  6500  manipulator. 

The  task  of  integrating  a  system  like  the  Utah/MIT  dexterous  hand 
to  a  robot  arm  is  kinematically  complex,  especially  in  light  of  the  fact 
that  these  two  systems  are  a  major  part  of  a  remote  teleoperation 
system.  The  Merlin  robot  arm  is  kinematically  unlike  the  human  arm 
while  the  Utah/MIT  hand  differs  from  the  human  hand  in  some  aspects, 
including  the  positioning  of  the  thumb  and  the  number  of  digits.  This 
makes  the  task  of  comparison  between  the  human  system  and  the  slave 
systems  a  difficult  process  at  best.  The  integration  task  is  further 
complicated  by  the  presence  of  the  remotizer,  which  performs  the 
function  of  locating  the  actuators  of  the  Utah/MIT  dexterous  hand  away 
from  the  physical  hand  itself.  These  complications,  amongst  others, 
result  in  the  need  for  a  complete  kinematic  understanding  of  the  Merlin 
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nanipulator  as  well  as  the  dexterous  hand,  as  well  as  a  means  for  the 
depiction  and  determination  of  possible  complications  that  may  arise 
when  the  two  slave  sub- systems  are  attached  together. 

is  a  first  step  in  aiding  the  iilRL  in  this  research  task,  the 
authors  have  performed  a  complete  kinematic  study  of  the  Merlin  6500 
robot  arm  and  the  direct  kinematics  of  the  UTAH/IIT  dexterous  hand. 
This  study  has  been  performed  with  the  basic  fact  in  mind  that  a  human 
arm  will  be  involved  in  the  feedback  loop  and  will  be  directing  the 
robot /dexterous  hand  combination  in  performing  tasks  with  the 
teleoperated  system.  Further,  to  study  the  problem  of  attachment  of  the 
Utah/IIT  hand  to  the  Merlin,  the  authors  have  developed  a  computer 
graphical  simulation  program  that  allows  a  user  to  study  different 
attachment  schemes  and  the  effect  that  these  schemes  may  have  on  the 
kinematic  behavior  of  the  slave  system. 

The  Objective 

The  objective  of  this  project  is  to  develop  the  closed- form  forward 
and  inverse  kinematic  solutions  of  the  Merlin  6500  robot  arm  and  the 
closed- form  forward  kinematic  solution  of  the  UTil/MIT  dexterous  hand. 
A  computer  graphical  simulation  of  the  two  systems,  when  connected 
together  in  user- defined  configurations,  is  also  performed  in  this 
study.  The  aim  of  the  computer  graphic  simulation  is  to  visually  depict 
the  effect  of  different  attachment  schemes  on  the  kinematic  behavior  of 
the  slave  sub- system  when  combined  together  in  specific  user- specif ied 
configurations,  and  to  prepare  the  ground- work  for  future  research  with 
the  remote  teleoperated  systems. 

Some  Important  Factors 

One  factor  kept  in  mind  during  the  development  of  computer 
simulations  was  the  need  for  the  software  to  be  transportable  to  the 
sponsor’s  systems.  A  second  factor  was  the  need  to  allow  for  changes 
when  adapting  the  simulation  to  the  sponsor’s  available  graphics 
packages.  As  a  result,  all  source  code  was  written  in  a  modular  fashion 
in  a  commonly  used  language  (FOETRAN),  and  utilizes  as  few  routines  out 
of  the  graphics  package  (DISSPLA)  as  possible.  The  simulations  are 
user-friendly  and  provide  for  modification  and  development  as  the 
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teleoperation  study  proceeds.  An  important  factor  considered  during  the 
development  of  the  kinematic  equations  was  that  the  robot/hand 
combination  would  be  driven  from  a  remote  location  by  a  human  arm 
encased  in  an  exo- skeleton.  This  resulted  in  the  kinematics  study  being 
performed  using  kinematic  frames  that  could  be  compared  to  a  human 
arm/hand  system. 
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II  BACKGROUND 


The  Need  for  a  Teleoperative  System 

Since  the  beginning  of  the  present  decade,  it  has  been  found 
necessary  to  perform  manipulations  in  environments  unsuitable  for  the 
presence  of  a  human  being.  Some  such  hazardous  environments  include 
radiation  hazard  zones,  chemical  or  biological  hazard  areas,  undersea, 
deep  space,  etc.  A  human  being  would  find  it  extremely  hazardous,  if 
not  impossible,  to  exist  in  such  environments,  and  since  it  is  necessary 
to  project  human  judgement  and  adaptability  to  perform  unstructured 
tasks  which  require  dexterous  manipulation,  there  exists  a  need  for 
remotely  operated  dexterous  systems  which  provide  a  means  for  projecting 
human  cognitive  and  motor  functions  into  such  environments.  Such 
systems,  when  fully  developed,  will  allow  an  operator,  present  at  a 
comparatively  safe  location,  to  perceive  and  perform  manipulation  tasks 
just  as  if  the  operator  was  physically  present  at  the  remote  work  site. 

Current  Work 

To  achieve  the  above  objective,  various  teleoperation  systems 

dedicated  to  performing  tasks  in  specific  hostile  environments  have  been 

developed  over  the  last  twenty  years.  Under- sea  teleoperative  systems 

have  been  successfully  used  to  perform  dexterous  manipulations.  The 

recent  Titanic  exploration  performed  by  the  Voods  Hole  Oceanographic 

Institute  using  the  manned  submersible  "Alvin"  and  the  tethered 

manipulator,  "Jason,  Jr"^  is  one  example.  Other  efforts  include  the 

Advanced  Integrated  Eanipulation  System  (AIMS  -  a  prototype 

remote- handling  system  for  use  in  hazardous  environments  developed  by 

the  Oak  Ridge  National  Laboratory)  where  the  master  arms  are  kinematic 

2 

replicas  of  the  slave  arms  ;  the  U.S.  Army’s  Human  Engineering 

o 

Laboratory  Soldier  "cbot  Interface  Project  (SHIP)  ,  meant  for 

battlefield  scenarios;  the  ORNL/NASA  Ian- Equivalent  Tele- Robot  (lETR), 

which  is  a  modularized  seven  degree- of- freedom  manipulator  ;  the  Remote 

Operations  and  Maintenance  Demonstration  (ROMD)  consisting  of  the  model 

M_2  manipulator  (a  dual- arm  force- reflecting  bilateral  servo- manipulator 
2 

system)  ;  the  Marine  Corps/Raval  Ocean  Systems  Center’s  (NOSC) 

2 

Cround-Rir  TRleRobotics  Systems  (CATERS)  ;  and  the  current  work  being 
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done  in  other  countries  with  aultiple  prehension  manipulator  systems  in 

q  A 

tele- robotic  applications  *  .  ill  these  teleoperative  systems  have  been 
designed  to  be  able  to  perform  dexterous  manipulation  tasks  in  specific 
environments . 

The  PTAH/MIT  Dexterous  Hand 

The  development  of  anthropomorphous  systems  for  bio- engineering 
applications  has  resulted  in  research  efforts  being  directed  towards  the 
implementation  of  dexterous  systems  which  could  be  utilized  for  a  large 
variety  of  manipulation  tasks.  The  existence  of  a  naturally  occurring, 
highly  complex  system  like  the  human  hand  has  led  to  the  development  of 
semi- anthropomorphic,  dexterous  manipulator  end- effectors.  One  such 
system  in  current  existence  is  the  UTAH/lIT  dexterous  hand,  a  sixteen 
degree- of- freedom  system  consisting  of  three  four- jointed  fingers  and  a 
four- jointed  thumb  situated  off-center  in  the  palm.  A  left-  and  right- 
pair  of  these  hands  will  be  used  at  AAllL  as  a  research  testbed  to 
experimentally  investigate  the  various  issues  associated  with 
human- in- the- loop  control  of  dexterous  end- effectors . 

The  Merlin  6500  Robot 

The  Utah/lIT  dexterous  hand  will  be  attached  to  the  lerlin  6500  six 
degree- of- freedom  robot  arm  to  form  the  remote  manipulator  system. 
Considerations  such  as  the  payload  capacity,  maximum  tool- tip  speed, 
accuracy  and  repeatability  when  encumbered  by  the  heavy  dexterous 
hand/remotizer  system,  the  primary  cost,  and  the  availability  of 
sufficient  degrees  of  freedom  to  allow  dexterous  operation,  etc. 
affected  the  choice  of  the  manipulator  for  the  purpose  of  evaluating  the 
feasibility  of  integrating  the  Utah/lIT  hands  to  a  robot  arm. 

The  lerlin  6500  robot  arm  is  a  six  degree- of- freedom  industrial 
manipulator  with  a  payload  capacity  of  50  lbs.  and  a  reach  of  40  inches. 
The  repeatability  of  the  lerlin  6500  arm  is  ^0.001  inches.  Each  of  the 
six  degrees- of- freedom,  viz.  the  waist,  shoulder,  elbow,  wrist  roll, 
wrist  pitch  and  hand  roll  are  controllable  through  a  digital  computer. 
The  lerlin  arm  is  therefore  well  suited  to  the  task  of  moving  around  in 
three-dimensional  space  with  the  Utah/lIT  hands  attached  at  the  end. 
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Issues 

The  first  issue  that  arises  when  linking  a  robot  arm  to  an 
end- effector  system  is  the  fact  that  the  two  systems  have  to  be  combined 
together  physically  to  be  able  to  perform  a  set  of  tasks.  The  next 
issue  that  must  be  addressed  is  the  availability  of  a  suitable 
work- space  provided  by  the  combined  systems,  such  that  performance  of 
the  desired  tasks  when  combined  together  would  not  be  inhibited.  Both 
issues  require  the  robot  and  end- effector  to  be  kinematically  understood 
and  accurately  modeled. 

The  actual  combination  of  the  Utah  /  IIT  dexterous  hand  to  the 
Berlin  robot  arm  is  complicated  by  the  presence  of  a  "remotizer",  a 
multiple- bar  linkage  mechanism  that  allows  the  pneumatic  actuation 
system  of  the  Utah/MIT  hand  to  be  located  away  from  the  physical  hand. 
This  study,  however,  does  not  deal  with  the  remotizer  in  any  way  beyond 
the  acknowledgement  of  its  presence  as  a  constraint  in  the  achievement 
of  anthropomorphic  arm  geometry  as  regards  the  robot/dexterous  hand 
combination.  This  study  is  intimately  concerned  with  the  kinematics  of 
the  two  remote  systems,  specifically,  with  the  direct  and  inverse 
kinematics  of  the  Merlin  6500  robot  arm  and  the  direct  kinematics  of  the 
Utah/llT  dexterous  hand. 

It  is  necessary  to  study  the  best  possible  method  of  attachment  of 
the  Utah/MIT  hand  to  the  Merlin  robot  arm.  It  is  also  needed  to  study 
the  behaviour  of  the  two  systems  when  combined  together  and  to  obtain  an 
idea  of  the  attachment  component  for  the  two  systems.  This  can  either 
be  done  using  actual  models  of  the  robot  arm,  the  dexterous  hand  and 
suitable  attachment  pieces,  or  can  be  performed  using  a  computer 
graphical  simulation,  or  both.  The  computer  graphical  simulation  method 
offers  the  advantage  of  being  less  costly  and  allows  for  many  more 
possible  kinematic  attachment  methods  to  be  studied.  The  simulation  can 
also  be  used  to  study  the  movement  of  the  manipulator  and  end- effector, 
as  well  as  assist  in  modeling  the  system’s  kinematic  behaviour.  Vith 
this  fact  in  mind,  a  computer  graphic  simulation  has  been  developed  to 
model  the  behaviour  of  the  left- shouldered  Merlin  arm  and  the 
left- fingered  UTAH/MIT  dexterous  hand.  A  minimum  set  of  commands  from 
the  graphics  package  (DISSPLA)  have  been  used  to  allow  for 
transportability  of  the  software  to  the  sponsor’s  site. 
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Ill  SPiTIU  TIANSFORIATIONS 


Descriptions 

lobotic  Banipulation  requires  that  the  end- effector  be  moved  around 
in  space.  This  involves  describing  positions  and  orientations  of  the 
■echanisa  in  a  aathenatical  fora.  The  definition  of  manipulator 
position  and  orientation  and  the  manipulation  of  mathematical  quantities 
which  represent  position  and  orientation  is  performed  by  using 
coordinate  systems  (or  frames)  and  transformations .  which  contain  the 
description  of  both  positions  and  orientations. 

Description  of  a  Position 

The  position  of  any  point  P  in  the  universe  can  be  represented  with 
respect  to  a  base  frame  by  a  [3x1]  position  vector.  As  different 
coordinate  systems  can  be  used,  vectors  must  be  tagged  with  information 
identifying  which  coordinate  system  they  are  described  in.  A  leading 
superscript  for  a  vector  indicates  the  coordinate  system  in  which  it  is 
referenced,  for  example,  P  refers  to  the  position  of  point  P,  which  is 
described  by  three  numerical  values  indicating  distances  along  the  axes 
of  frame  {A}.  Individual  components  of  a  vector  are  identified  by  the 
trailing  subscript  x,  y  and  z.  Thus,  the  positional  representation  of 
point  P  relative  to  {A}  would  be  written  as 

r  nT 


where  T  denotes  the  transpose  of  the  matrix. 

Description  of  an  Orientation 

The  compleU  location  of  a  body  in  space  is  not  specified  until  its 
orientation  is  also  given.  A  point  on  a  body  could  be  oriented 
arbitrarily  while  being  at  the  same  position  with  respect  to  the  base 
frame.  To  describe  the  orientation  of  a  body,  we  attach  a  coordinate 
system  {B}  to  the  body  and  then  give  a  description  of  the  coordinate 
system  relative  to  the  reference  system  {A}. 
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Thus,  positions  of  points  are  described  with  [3x1]  vectors,  while 
orientations  of  bodies  are  described  by  body- attached  coordinate 
systems.  One  convenient  way  to  describe  the  body- attached  coordinate 
system  is  to  describe  the  unit  vectors  of  its  three  principal 
(orthonormal)  axes  in  terms  of  the  unit  vectors  in  the  universe  (or 
base)  coordinate  system.  It  must  be  noted  here  that  the  description  of 
two  vectors  would  suffice,  since  the  third  can  be  obtained  by  taking  the 
cross-product  of  the  given  two.  The  unit  vectors  along  the  principal 
directions  of  the  body- attached  coordinate  frame  {B}  can  be  denoted  as 
Ig,  Yg  and  Zg.  Vhen  written  in  terms  of  the  universe  or  base  coordinate 

system  {A},  these  vectors  are  written  as  ^Xg,  ^Yg,  and  ^Zg.  It  is 

convenient  to  stack  these  unit  vectors  together  as  the  columns  of  a 
[3x3]  matrix,  in  the  order  ^Ig,  ^Yg  and  ^Zg.  This  [3x3]  matrix  is  the 

rotation  matrix  which  describes  {B}  relative  to  {A}  and  is  written  as 

A  A 

gR.  Explicitly,  gR  is  given  by 


^R  - 
B*^  - 

Ay  Ay  An 

*B  *B  ^B 

II 

'll 

'21 

'l2 

'22 

'l3 

'23 

- 

'31 

'32 

'33 

Description  of  a  Transformation 

The  information  needed  to  completely  specify  the  whereabouts  of  the 
manipulator  end- effector  is  its  position  and  orientation.  The  point  on 
the  body  whose  position  is  chosen  to  be  described  is  the  origin  of  the 
body- attached  frame  {B}.  The  position  and  orientation  pair  which 
completely  describes  a  body’s  whereabouts  is  combined  together  to  form  a 
transformation,  which  is  defined  as  a  set  of  four  vectors  giving 
position  and  orientation  information.  It  must  be  remembered  here  that  a 
frame  is  an  orthogonal  coordinate  system  which  is  described  relative  to 
some  other  frame.  Thus,  when  the  frame  {B}  is  described  with  respect  to 
the  frame  {A},  then  gT  can  be  represented  as 


(3.3) 
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where  gR  is  the  rotation  aatrix  representation  of  {B}  relative  to 

{A},  and  is  specified  by  equation  (3.2),  and  is  the  vector  fron 

org 

the  origin  of  {A}  to  the  origin  of  {B}  and  can  be  written  according  to 
equation  (3.1). 

Transforaations;  Changing  Descriptions  from  Fraae  to  Prane 

In  robotic  kinenatics,  we  are  concerned  with  describing  position 
and  orientation  in  various  reference  coordinate  systens.  Thus,  we  need 
to  be  able  to  transfom  this  inforaation  fron  fraae  to  fraae  rather 
frequently. 

Translated  Frames 

Let  the  position  of  the  point  P  be  defined  with  reference  to  the 

fraae  {B}  as  shown  in  Figure  1.  It  is  required  to  express  the  position 

of  P  with  respect  to  {A}.  Vhen  {A}  has  the  sane  orientation  as  {B},  the 

difference  in  {A}  and  {B}  can  be  represented  by  a  translation  vector, 

^P|.  ,  which  locates  the  origin  of  {B}  with  respect  to  {A}, 

org 


Figure  1.  Translational  lapping? 
{B}  has  the  saae  orientation  as  {A}. 
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Since  both  vectors  are  defined  relative  to  frames  of  the  same 
orientation,  we  can  compute  the  description  of  point  P  relative  to  {A} 
by  the  use  of  vector  addition: 

=  »P  .  \  (3.4) 

org 

It  must  be  remembered  that  it  is  possible  to  add  vectors  that  are 

defined  in  terms  of  different  frames  only  if  the  frames  have  the 

same  orientation.  It  must  also  be  noted  here  that  the  point  P  has 

itself  not  moved  in  space  -  only  its  description  has  changed. 

The  vector  ^Pn  defines  a  translational  mapping  of  point  P  from 
org 

its  description  in  {B}  to  {A},  since  all  the  information  needed  to 

perform  the  change  in  description  is  contained  in  ^Pg  (along  with  the 

org 

knowledge  of  their  equivalent  orientation). 

Rotated  Frames 

The  matrix  gR  describes  the  relative  orientation  of  {B}  with  {A} 

and  is  composed  of  the  three  column  vectors,  ^Ig,  ^Yg  and  ^Zg.  By  our 

definition,  the  columns  of  a  rotation  matrix  have  unit  magnitude  and 
represent  vectors  that  are  orthonormal.  Since  the  inverse  of  a  matrix 
with  orthonormal  columns  is  equal  to  its  transpose,  we  have 

Jr  =  Jr-'  =  Jr^  (3.5) 


Thus,  since  the  column  vectors  of  Jr  arc  the  unit  vectors  of  {B} 
written  in  {A},  the  rows  of  Jr  are  the  unit  vectors  of  {A}  written  in 
{B}. 

As  such,  a  rotation  matrix  can  be  interpreted  as  a  set  of  three 
column  vectors  or  as  a  set  of  three  row  vectors  as  follows: 


BtT  ByT  B„T 
^A  *A  ^A 


T 


(3.6) 
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Ve  often  need  to  know  the  coaponents  of  a  vector  vith  respect  to  a 
fraae  {A}  when  ve  know  its  components  vith  respect  to  a  frame  {B},  where 
the  origins  of  frame  {A}  and  {B}  are  coincident  (Figure  2).  This 
computation  is  possible  when  a  description  of  the  orientation  of  {B}  is 
known  vith  respect  to  {A}.  This  orientation  is  given  by  the  rotation 
matrix  qK. 


{B\  {i4} 
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Figure  2.  Rotated  frames. 


Since  the  components  of  any  vector  are  simply  the  projections  of 
that  vector  onto  the  unit  directions  of  its  frame,  the  projection  is 
computed  by  the  vector  dot  product.  Thus,  the  components  of  can  be 
computed  as 


(3.7) 


In  order  to  express  the  above  equation  in  terms  of  a  rotation 

1 

i 
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matrix  multiplication,  we  note  from  the  previous  equation  that  the  rows 
A  B  R  R 

of  are  and  As  such,  the  above  equation  can  be  written 

compactly  as 

=  gR  ®P  (3.8) 

Equation  (3.8)  implements  a  rotational  mapping  from  frame  {B}  to 

B  A 

frame  {A},  i.e.  it  changes  the  description  of  a  vector  from  P  into  P. 
Mappings  Involving  General  Frames 

Ve  can  now  address  the  problem  of  mappings  involving  general 

frames,  i.e.  those  frames  where  both  translational  and  rotational 

differences  are  involved.  In  this  case,  the  frames  {A}  and  {B}  do  not 

have  coincident  origins,  nor  do  they  possess  equivalent  orientations. 

The  vector  that  locates  {B}’s  origin  relative  to  {A}  is  called  ^Pg  , 

org 

while  the  rotation  of  frame  {B}  relative  to  {A}  is  given  by  gR.  Given 

n 

P,  the  vector  describing  the  point  P  with  respect  to  frame  {B},  we  wish 
to  compute  ^P,  the  description  of  the  vector  relative  to  {A}. 

|l 

This  is  done  by  changing  P  to  its  description  relative  to  an 
intermediate  frame  which  has  the  same  orientation  as  {A},  but  whose 
origin  is  co- incident  to  {B}.  This  is  mathematically  performed  by 
pre- multiplying  P  by  gR,  as  seen  previously  in  (3.8).  Ve  can  now 

translate  between  origins  by  performing  simple  vector  addition,  since 
the  intermediate  frame  and  {A}  have  equivalent  orientations. 
Mathematically  speaking,  this  is  done  as  follows: 

^P  =  Jr  ®P  +  hr.  (3.9) 

org 

The  above  equation  describes  a  general  transformation  of  a  vector 
from  its  description  in  one  frame  to  its  description  in  another. 

Since  we  are  also  interested  in  a  concise  notation,  the  above 
equation  can  be  written  as 
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(3.10) 


i 


P 


where  the  operator  ^  is  defined  by 


A 

B» 

,  (3x1) 

®org 

0  0  0 

1 

4x4 


(3.11) 


A  B 

and  the  P  and  P  vectors  are  embedded  in  [4x1]  matrices. 

The  4x4  matrix  in  (3.11)  is  called  the  homogeneons  transformation 
operator.  This  transformation  matrix  consists  of  the  position  and 
orientation  sub- matrices  and  represents  a  description  of  the  frame  {B} 
relative  to  frame  {A}  as  well  as  the  transformation  of  a  vector 
described  in  terms  of  frame  {B}  to  its  description  in  {A}. 


The  Mathematics  of  Transformation  Operators 

Before  we  proceed  farther,  it  is  advisable  to  explain  the  two 
important  mathematical  operations  in  manipulator  kinematics  regarding 
the  transformation  operator  gT,  viz.  concatenation  and  inversion. 

Multiple  transformations  are  performed  when  there  exist  more  than 
two  frames  and  one  of  the  frames,  say  {C}  (or  a  vector  represented  by 

one  of  the  frames),  needs  to  be  mapped  to  the  first  frame  {A}  through 

the  second  frame  {B}.  This  situation  is  encountered  when  the  available 
description  of  the  frames  includes  a  description  of  the  third  frame  {C} 

D 

relative  to  the  second  one  {B},  i.e.  ^T  is  known,  and  the  second  frame 

{B}  relative  to  the  first  frame  {A},  i.e.  gT  is  known.  The  compound 

transformation  is  mathematically  performed  by  the  use  of  matrix 
multiplication  operations  as  follows 


(3.12) 


Here, 


represents  the  homogeneous  transform  or  mapping  of  frame 
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{C}  with  respect  to  frame  {A}.  Ve  notice  the  notational  convenience 
here  -  the  leading  sub- script  of  the  first  term  on  the  right  side  of  the 
above  equation  may  be  said  to  "cancel”  the  leading  super- script  in  the 
second  term  on  the  right  side  of  the  equation,  to  give  the  term  on  the 
left  side  of  the  equation. 

In  many  cases,  it  is  necessary  to  perform  a  transformation  matrix 
inversion.  Typically,  this  is  done  where  the  order  of  frame 
descriptions  is  found  to  be  incompatible  with  compound  transformation 

proceedures.  In  the  example  given  above,  if  the  description  of  {B} 

C  A 

relative  to  {C},  i.e.  gT  was  known,  then  the  determination  of  could 

only  be  performed  by  inverting  gT  to  obtain  ^T,  and  using  the  above 

equation  to  determine  ^T.  Thus,  (3.12)  will  now  become 


A 

C 


T 


(3.13) 


The  inversion  of  the  transformation  matrix  could  be  easily 
performed  by  the  generalized  matrix  inversion  method.  A  computationally 
faster  method  (involving  a  fewer  number  of  operations)  and  one  which 
utilizes  the  inherent  structure  (orthogonality)  of  the  rotation  matrix 

to  advantage  is  explained  below. 

R  B  R  A  A 

To  find  ?T,  we  must  compute  and  P.  from  gR  and  Pg  .  From 

org  ®org 

(3.5),  we  have 

Jr  =  gR"^  (3.14) 

and  so  we  change  the  description  of  ^Pg  into  {B}  using  the 

org 

transformation  involving  general  frames,  as 


"(V 


B  ^ 
org 


(3.15) 


Since  the  left  side  of  the  above  equation  is  necessarily  zero,  we  have 


(3.16) 


org 


A*  • 


org 


¥  h 

B*  •  ‘^B 


org 


Ve  can  therefore  write  ^  gT  i.e.,  ®T  as: 


-  - 
■  A^  " 

A»T 

b‘ 

A„T  Ap 

B*  *  *^B 

org 

0  0 

0 

1 

(3.17) 


Ve  can  thus  perfon  the  inversion  operation  on  the  transform  matrix 
using  (3.17). 
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IV  MANIPULATOR  KINEMATICS 


Manipulator  kinematics  defines  the  geometrical  properties  of 
motion.  The  direct  kinematics  problem  is  defined  as  the  determination 
of  the  end- effector  position  and  orientation  when  the  joint  variables 
are  known  [Appendix  1.7],  while  the  inverse  kinematic  problem  is  defined 
as  the  determination  of  the  joint  variables  to  achieve  the  desired 
position  and  orientation  [Appendix  1.8].  Ve  will  first  examine  the 
generalized  direct  kinematic  problem,  followed  by  a  study  of  certain 
important  factors  involved  in  the  generalized  inverse  manipulator 
kinematics  problem. 

In  order  to  deal  with  the  complex  geometry  of  a  manipulator,  frames 
are  affixed  to  various  parts  of  the  mechanism.  Vhen  the  mechanism 
articulates,  the  relationship  between  the  frames  describes  the  kinematic 
behaviour  of  the  manipulator. 

Joint  Description 

A  manipulator  consists  of  a  set  of  links  connected  together  in  an 
open  chain  by  joints. 

Types  of  Manipulator  Joints 

Manipulator  links  can  be  joined  together  by  a  variety  of  joint 

a 

types.  The  commonly  existing  manipulator  joint  types  consist  of  : 

Revolute  joint,  where  the  joint  consists  of  a  simple  hinge,  with 
the  only  possible  relative  motion  between  the  paired  members  being  a 
rotation  about  the  joint  axis.  This  is  the  most  commonly  used  joint  in 
manipulators. 

Prismatic  joint,  where  the  joint  consists  of  a  sliding  type 
mechanism,  with  no  relative  rotation  occurring  between  the  jointed 
members.  The  only  possible  relative  motion  is  a  pure  (rectilinear) 
translation  along  the  slide  direction.  This  is  the  next  most  commonly 
used  joint  in  manipulators. 

Helical  joint.  These  are  rarely  found  in  manipulators  due  to  the 
difficulty  in  powering  the  joint.  The  effect  of  a  helical  joint  is 
normally  obtained  by  a  special  combination  of  the  revolute  and  prismatic 
joints.  The  joint  acts  like  a  screw- and- nut  arrangement.  It  can  be 


substituted  for  by  a  co-axial  revolute  and  prismatic  joint  with  a 
constant  ratio  of  rotational  to  translational  displacement. 

Cylindrical  joint,  which  is  in  effect  a  revolute  joint  without  the 
end  constraints,  i.e.,  sliding  takes  place  along  the  revolute  axis. 
This  joint  is  normally  found  in  manipulators  as  a  co-axial  revolute  and 
prismatic  joint,  with  each  joint  independantly  powered  and  controlled. 

Spherical  joint,  which  consists  of  a  spherical  ball  and  socket 
arrangement.  The  relative  motion  is  spherical,  resulting  in  all  points 
remaining  at  a  fixed  distance  from  the  center  point  of  the  joint.  In 
manipulators,  the  effect  of  this  joint  is  obtained  by  three  non-coplanar 
independently  powered  revolute  joints  whose  axes  always  intersect  at  a 
point. 

Flat  planar  joint,  which  consists  of  two  flat  nlanes  sliding  and 
turning  on  each  other.  It  can  be  kinematically  constructed  by  two 
non- planar  prismatic  joints  and  a  revolute  joint  perpendicular  to  the 
directions  of  both  the  prismatic  joints. 

Although  other  manipulator  joints  do  exist,  they  are  rarely  used 
due  to  the  associated  problems  in  powering  and  controlling  them. 

In  certain  cases,  as  in  some  of  those  above,  there  exist 
manipulator  joints  with  more  than  a  single  degree  of  freedom.  These 
joints  can  be  kinematically  modelled  as  ’n’  joints  of  one  degree  of 
freedom  each,  connected  together  with  ’n-1’  links  of  zero  link  length. 
As  such,  we  will,  without  loss  of  generality,  consider  manipulator 
kinematics  with  joints  having  single  degrees  of  freedom  at  each  joint. 

Significant  Dimensions  of  Joints 

Significant  dimensions  for  joints  consist  of  the  link  offset  (d^) 

and  the  joint  angle  (^^).  Neighbouring  links  are  joined  together  at  any 

one  joint,  which  has  an  axis  of  motion  that  is  common  to  both  the  links 
connected  at  the  joint.  The  distance  along  this  common  axes,  from  one 
link  to  the  next,  is  called  the  link  offset  (d^).  The  link  offset  d^ 

for  joint  i  is  thus  the  distance  measured  along  the  axis  of  joint  i, 
from  the  intersection  of  the  common  perpendicular  between  the  axes  of 
joints  i- 1  and  i,  to  the  intersection  of  the  common  perpendicular 
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between  the  axes  of  joints  i  and  i+1.  The  joint  angle  9^  describes  the 

amount  of  rotation  about  the  common  axes  at  the  joint,  between  one  link 
and  its  neighbour.  This  parameter  is  measured  as  the  angle  from  the 
extension  of  the  common  perpendicular  between  the  axes  of  joint  i  and 
i- 1  to  the  common  perpendicular  between  the  axes  i  and  i+1,  in  a  plane 
perpendicular  to  the  axis  of  joint  i.  The  link  offset  is  considered  to 
be  the  joint  variable  if  the  joint  under  consideration  is  prismatic  in 
nature,  while  the  joint  angle  is  the  joint  variable  if  the  joint  under 
consideration  is  revolute. 


Figure  3.  Link  and  joint  parameters^ 


LINK  DESCRIPTION 

Ve  now  examine  the  significance  and  kinematic  representation  of 
links  as  well  as  their  description. 
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The  Significance  and  Kinematic  Representation  of  Links 

Links  are  used  to  connect  joints.  The  kinematic  significance  of 
links  is  that  they  maintain  fixed  configurations  between  their  joints 
and  other  points  and  lines  along  the  axis  of  the  joints.  It  is 
important  to  note  here  that,  regardless  of  the  actual  location,  shape  or 
size  of  a  link,  a  manipulator  may  be  completely  represented 
kinematically  by  a  skeleton  diagram,  which  is  a  line  drawing 
representation  of  the  links  of  the  manipulator. 

Significant  Link  Dimensions 

Significant  dimensions  of  a  link  consist  of  the  link  length  and  the 
link  twist.  For  any  two  joint  axes  in  three  dimensional  space,  there 
exists  a  well-defined  measure  of  distance  between  them.  The  distance 
measured  along  a  line  which  is  mutually  perpendicular  to  both  axes 
defines  the  link  length.  The  link  twist  is  measured  in  a  plane  whose 
normal  is  the  mutually  perpendicular  line  between  the  two  axes  (the  axes 
under  consideration  and  the  proceeding  joint  axes)  and  is  defined  by  the 
angle  formed  between  the  projections  on  this  plane  of  the  two  joint  axes 
(see  figure  3). 

Any  open  kinematic  chain  can  be  described  by  specifying  the  values 
of  the  joint  angle,  link  offset,  link  length  and  link  twist  for  each 
joint- link  system.  Of  these  four  parameters,  three  are  constant  for  a 
joint,  while  the  fourth  parameter  forms  the  joint  variable.  The 
specification  of  an  open  kinematic  chain  by  means  of  these  four 

c  7 

quantities  is  known  as  the  Denavit- lartenburg  convention 
The  Denavit-Hartenburg  Notation 

The  Denavit-Hartenburg  notational  convention  involves  the 
description  of  a  robot  arm  by  means  of  the  link  length  a^^  link  twist 

angle  link  offset  d.,  and  the  joint  angle  9^.  The  method  depends 

on  the  fixing  of  a  frame  to  each  joint  of  the  robot  and  determining  the 
joint  parameters  and  joint  variable  range.  Ve  utilise  the  convention 
that  frame  {i}  has  its  origin  at  joint  axis  i  and  is  attached  to  link  i. 
Thus,  the  parameter  link  length  (a-  j)  is  measured  as  the  signed 

distance  along  the  common  perpendicular  to  the  axes  i-1  and  i,  from 
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joint  axis  i-1  to  joint  axis  i.  The  link  tvist  is  measured  as 

the  signed  angle  (using  the  right-hand  rule)  between  the  projection  of 
axis  i-1  to  axis  i  on  a  plane  whose  normal  is  the  mutually  perpendicular 
line  between  axes  i-1  and  i.  The  link  offset  (dj)  is  the  signed 

distance  measured  along  the  axis  of  joint  i  from  the  point  where  a^  j 

intersects  the  axis  i,  to  the  point  where  a^  intersects  that  axis.  The 

joint  angle  is  measured  as  the  signed  angle  (using  the  right  hand 

rule)  between  the  extension  of  a^  j  and  a^,  about  the  axis  of  joint  i. 

In  the  special  case  of  the  joint  being  the  first  one  under 

consideration,  i.e.  i  is  1,  the  link  parameters  are  determined  from  the 
base  frame,  here  (i-1)  is  0.  Since  link  length  a^  and  link  twist 

depend  on  joint  axes  i  and  i+1,  the  parameters  at  the  end  of  the  chain, 

a  and  a  ,  are  set  to  0  and  do  not  need  to  be  defined, 
n  n' 

Affixing  Frames  to  Links 

In  order  to  describe  the  location  of  each  link  relative  to  its 

neighbours,  a  frame  is  attached  to  each  link.  The  link  frames  are  named 
according  to  the  link  to  which  they  are  attached,  i.e.  frame  {i}  is 
rigidly  attached  to  link  i. 

The  convention  adopted  for  affixing  frames  to  links  depends  on 

whether  the  link  is  an  intermediate  link  or  the  first/last  link  in  the 
chain. 

First  and  Last  Links  in  the  Chain 

Ve  attach  the  frame  {0}  to  the  base  of  the  robot,  or  to  a 

non- moving  section  of  the  arm,  called  link  {0}.  This  base,  or  reference 
frame,  can  also  be  set  up  with  its  origin  coinciding  with  frame  {1}  when 
the  joint  1  variable  is  0  (the  generally  preferred  method).  The  Z-axis 
of  frame  {0}  coincides  with  the  Z-axis  of  frame  {1},  and  so  do  the  X  and 
Y  axes.  This  ensures  that  aQ  =  0.0  and  Oq  =  0.0.  Additionally, 

dj  =  0.0  if  joint  1  is  revolute,  while  0^  =  0.0  if  joint  1  is  prismatic. 

However,  when  the  base  or  reference  frame  is  not  located  to  coincide 
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with  frame  {!},  ^  0.0  and  Oq  #  0.0.  In  this  case,  it  is  not 

necessary  that  0^  be  equal  to  0.0.  The  base  frame  {0}  is  then  set  up 
for  mere  convenience. 

For  joint  *n’  revolute,  the  direction  of  is  chosen  so  that  it 

aligns  with  1^  ^  when  <7^  =  0.0,  and  the  origin  of  frame  {N}  is  chosen  so 

that  d^  =  0.0.  In  cases  where  three  axes  intersect  at  a  point,  frame 

{N}  is  located  at  the  point  of  intersection  of  the  three  axes.  If  joint 
’n’  is  prismatic,  the  direction  of  is  chosen  so  that  0^  =  0.0  and  the 

origin  of  frame  {N}  is  chosen  at  the  intersection  of  and  joint  axes 

’n’  when  d^^  =  0.0. 


Figure  4.  Link  frames  and  kinematic  parameters®. 


Intermediate  Links  in  the  Chain 

The  convention  used  to  affix  frames  on  intermediate  links  involves 
setting  the  Z-axis  of  frame  {i},  called  Z^,  coincident  with  the  joint  i 
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axis.  The  origin  of  frame  {i}  is  located  where  the  a.  perpendicular 

intersects  the  joint  i  axis.  The  direction  of  can  be  in  either 

direction  along  the  joint  i  axis.  is  set  up  so  that  it  points  along 

a.  in  the  direction  from  joint  i  to  joint  i+1.  In  the  special  case  of 

a^  =  0,  X|  is  chosen  normal  to  plane  of  and  The  link  twist 

is  measured  in  the  right  hand  sense  about  X^^.  is  formed  by  the  right 

hand  rule  to  complete  the  i^^  frame.  Figure  4  shows  the  location  of  the 
frames  and  the  kinematic  parameters. 

The  Link  Parameters  in  Terms  of  the  Link  Frames 

Attachment  of  the  link  frames  to  the  links  according  to  the 
convention  described  above  results  in  the  manipulator  kinematic 
parameters  being  redefined  in  terms  of  the  link  frames  as  follows  : 

aj^  =  the  signed  distance  from  to  measured  along  X^, 

=  the  signed  angle  between  Z^^  and  Z^^^,  measured  about  X^  in  the 

right  hand  sense, 

d^  =  the  signed  distance  from  X^_^  to  X^,  measured  along  Z^,  and 

0^  =  the  signed  angle  between  X^  ^  and  X^,  measured  about  Z^  in  the 

right  hand  sense. 

It  must  be  noted  here  that  the  above  convention  does  not  result  in 
a  unique  attachment  of  frames  to  links.  Vhen  the  Z^^-axis  is  aligned 

along  joint  axis  i,  there  are  two  choices  of  direction  in  which  to  point 
Z^.  Also,  in  the  case  of  intersecting  joint  axes  (i.e.  a^  =  0),  there 

are  two  choices  for  the  direction  of  X^,  corresponding  to  the  choice  of 

signs  for  the  normal  to  the  plane  containing  Z^  and  Z^^^. 

Derivation  of  Link  Transforms 

The  general  form  of  the  transformation  which  relates  the  frames 

attached  to  neighbouring  links  is  now  derived.  These  transformations 


22 


are  then  concatenated  to  solve  for  the  position  and  orientation  of  link 
’n*  relative  to  link  0. 


5 

Figure  5.  Intermediate  link  frames  and  kinematic  parameters  . 

The  determination  of  the  transformation  which  defines  frame  {i} 
relative  to  frame  {i+1}  is,  in  general,  a  function  of  the  four  link 
parameters.  For  any  given  robot  arm,  this  transformation  will  be  a 
function  of  only  one  variable,  the  other  three  being  fixed.  It  must  be 
remembered  here  that  we  are  dealing  with  multiple  degree- of- freedom 
joints  as  multiple  joints  with  one  degree  of  freedom  and  zero  offsets 
each.  By  defining  a  frame  for  each  link,  the  kinematic  problem  has  been 
broken  into  ’n’  sub- problems .  To  solve  each  of  these  sub- problems ,  it 
is  further  necessary  to  divide  them  further  into  four  sub- subproblems . 
Each  of  the  four  sub- subproblems  consists  of  a  basic  transformation 
which  is  a  function  of  one  link  parameter  and  can  be  written  by 
inspection. 

It  is  necessary  to  define  three  intermediate  frames  {P},  {Q}  and 
{E}  for  each  link.  Figure  5  shows  the  same  pair  of  joints  as  figure  4, 
with  the  intermediate  frames  {P},  {Q}  and  {E}  defined.  For  clarity. 


only  the  I  and  Z  axes  are  shown. 

In  figure  5,  frame  {R}  differs  from  frame  {i-1}  only  by  a  rotation 
of  Frame  {Q}  differs  from  {R}  by  a  translation  Frame  {P} 

differs  from  {Q}  by  a  rotation  and  frame  {i}  differs  from  {P}  by  a 

translation  d^.  To  write  the  transformation  which  transforms  vectors 

defined  in  {i}  to  their  description  in  {i-1},  we  write 

=  (^-1) 

or 

i-lp^i-llip  (4  2) 

where 

i-‘T  =  Jt  Ji  \t  (4.3) 

Equation  (4.3)  may  therefore  be  written  as 

Tniii8(Ij,a..j)  Eot(Zj,Pj)  Tran8(Zj,d.)  (4.4) 

or 

=  Screw(X^,  a^^  Screw(Z^,  d^,  0^)  (4.5) 

where  Screw(Q,r,^)  stands  for  a  translation  along  an  axis  Q  by  a 
distance  r,  and  a  rotation  about  the  same  axis  by  an  angle 

The  general  form  of  the  transformation  of  vectors  defined  in  frame 
{i}  to  their  description  in  frame  {i-1},  i.e.  ^^^T,  is  obtained  from 

(4.5)  (detailed  in  Appendix  2),  and  is  given  by 
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(4.6) 


The  Direct  Kinematics  of  Manipulators 

laving  derived  the  link  frames  and  the  corresponding  link 
parameters,  developing  the  direct  kinematic  equations  is  a 
straight-forward  process.  Using  the  values  of  the  link  parameters,  the 
individual  link  transform  matrices  are  computed.  The  manipulator  arm 
kinematic  transformation  matrices  are  then  multiplied  together  to  find 
the  single  transform  that  relates  frame  {N}  to  frame  {0},  as  shown  in 
equation  (4.7). 

»T  =  «I  >T  2, - »-,ll  (4.7) 


This  transformation  will  be  a  function  of  all  ’n’  joint  variables. 
The  kinematic  parameters  for  joint  ’i’  are  j  and  dj|^  as  well  as 

the  joint  variable  for  a  revolute  joint.  Each  of  these  parameters, 

as  well  as  the  joint  variable,  have  to  be  determined  for  each  link  of 
the  manipulator. 

The  Inverse  Kinematics  of  Manipulators 

The  inverse  kinematics  problem  involves  the  determination  of  the 
joint  angles  of  the  manipulator  which  will  achieve  the  desired  position 
and  orientation.  This  more  difficult  problem  can  be  solved  by  various 


25 


methods,  of  which  the  prominent  and  easily  programmable  ones  utilise 
either  geometric  or  algebraic  manipulations  to  obtain  a  set  of 
solutions.  One  of  the  important  factors  that  has  to  be  taken  into 
account  consists  of  whether  the  defined  (known)  position  is  at  the  tip 
of  the  manipulator  or  at  some  other  convenient  point  along  the  last 
axis.  Another  important  factor  to  be  taken  into  consideration  is 
whether  there  exists  a  solution  for  the  desired  position  and 
orientation.  Further,  the  solution  set  may  consist  of  one  or  more 
solutions  which  will  allow  the  achievement  of  the  desired  position  and 
orientation,  and  a  choice  between  these  solutions  must  be  made. 

Solvability 

The  problem  of  solving  the  kinematic  equations  of  a  manipulator  to 
determine  the  joint  angles  is  a  non-linear  one.  Given  the  values  of 
each  of  the  terms  in  ^T,  we  have  to  determine  a  viable  set  of  joint 

angles  $2^  ^3)  *  *  '  ^  degree- of- freedom  arm,  the  set 

of  joint  angles  that  needs  to  be  determined  (the  unknowns)  is  six.  Ve 
have  a  total  of  16  values  obtained  from  the  matrix,  four  of  which 

(the  last  row)  are  trivial  (equating  0  or  1  on  both  sidesl.  Out  of  the 
remaining  twelve  known  equations,  three  equations  define  the  position 
values  and  are  independant.  From  the  nine  remaining  equations  that 
arise  from  the  rotation  matrix  part  of  ^T,  only  three  equations  are 

independant.  These  three  equations,  added  with  the  set  of  three 
equations  that  arise  from  the  position  vector  part  of  the  transformation 
matrix,  provide  a  set  of  six  equations.  For  a  six  degree  of  freedom 
manipulator,  we  have  six  joint  angles  to  be  determined,  and  six 
equations.  These  equations  are  a  set  of  non-linear  transcendental 
equations  which  can  be  difficult  to  solve,  specially  for  a  general 
mechanism  with  six  degrees  of  freedom  with  all  link  parameters  non- zero. 
This  is  unlike  industrial  manipulators  where  the  link  parameters  consist 
of  twist  angles  of  0°  or  90°,  resulting  in  their  sine  and  cosine  values 
being  ’nice’  numbers  like  0  or  1,  or  where  many  of  the  offsets  are  0. 
As  with  any  set  of  non-linear  equations,  it  is  necessary  to  look  for  the 
existence  of  solutions,  multiple  solutions  and  the  method  of  solution. 
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The  Existence  of  Solutions  and  Manipulator  Workspaces 

The  question  of  whether  or  not  there  exists  an  inverse  kineaatic 
solution  for  the  successful  achieveaent  of  the  desired  position  and 
orientation  raises  the  question  of  manipulator  workspace.  Broadly 
speaking,  workspace  is  that  volume  of  three  dimensional  space  which  the 
end- effector  of  the  manipulator  can  reach.  For  a  solution  to  exist,  the 
desired  goal  point  must  lie  on  or  within  the  workspace  boundaries.  The 
dexterous  workspace  is  that  volume  of  space  which  the  robot  end- effector 
can  reach  with  all  orientations,  i.e.  at  each  point  in  the  dexterous 
workspace,  the  end- effector  can  be  arbitrarily  oriented.  The  reachable 
workspace  is  that  volume  of  space  which  the  robot  can  reach  in  at  least 
one  orientation.  Thus,  the  dexterous  work- space  of  a  robot  is  a  sub- set 
of  it’s  reachable  workspace. 

For  each  manipulator,  there  exists  an  outer  and  inner  workspace 
boundary.  Thus,  there  exists  an  outer  reachable  and  an  outer  dexterous 
workspace  boundary,  as  well  as  an  inner  reachable  and  inner  dexterous 
boundary.  The  outer  and  inner  workspaces  are  a  function  of  the 
kinematic  parameters  of  the  manipulator  and  the  joint  variable  range 
limits. 

The  Existence  of  Multiple  Solutions 

Another  common  problem  encountered  in  solving  manipulator  inverse 
kinematic  equations  is  that  of  multiple  solutions.  The  existence  of 
multiple  solutions  arises  due  to  the  kinematic  arrangement  of 
consecutive  joints  and  the  range  of  motion  of  each  joint.  For  example, 
when  there  exist  two  joints  with  successive  parallel  horizontal  axes, 
one  of  the  ways  to  achieve  the  desired  position  is  with  the  first  link 
pointing  upwards  with  the  second  link  pointing  downwards,  while  the  same 
position  is  achievable  by  the  first  link  pointing  downwards  and  the 
second  link  pointing  upwards.  Another  example  of  the  existence  of 
multiple  solutions  involves  the  orienting  mechanism  of  the  robot.  For 
each  solution,  provided  the  joint  variable  ranges  are  not  exceeded, 
there  will  exist  a  wrist  ’flipped’  solution.  Also,  the  more  nonzero 
link  parameters  there  exist  for  the  manipulator  arm,  the  more  ways  there 
will  be  to  achieve  the  desired  goal.  For  a  manipulator  with  six 
rotational  joints,  the  maximum  number  of  solutions  is  related  to  the 
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number  of  the  link  length  parameters  (a^)  that  are  equal  to  zero.  The 

more  that  are  nonzero,  the  bigger  the  number  of  solutions.  For  a 
completely  general  rotary- jointed  manipulator  with  six  degrees  of 
freedom,  there  are  up  to  16  solutions  that  are  possible.  Table  1  shows 
the  relationship  between  the  link  length  parameters  (a^^)  and  the  number 

of  solutions  for  a  six  degree  of  freedom  manipulator. 


5 

Table  1 .  Number  of  Solutions  vs.  Nonzero  aj^T 


»i 

Number  of  solutions 

<  4 

aj  =  aj  =  0 

00 

»3  '  " 

<  16 

All  a.  =  0 

<  16 

The  Methods  of  Solution 

Unlike  the  process  of  solving  a  system  of  linear  equations,  there 
are  no  general  algorithms  that  can  be  adopted  to  solve  a  set  of 
non-linear  equations.  It  therefore  becomes  necessary  to  note  that  a 
manipulator  is  considered  solvable  if  the  joint  variables  can  be 
determined  by  an  algorithm  which  allows  the  determination  of  all  the 
sets  of  joint  variables  associated  with  the  goal  frames  position  and 
orientation. 

The  broad  division  of  manipulator  solution  strategies  is  divisible 
into  closed- form  solutions  and  numerical  solutions.  Due  to  the 
iterative  nature  of  numerical  solutions,  they  are  much  slower  in 
"solving”  the  manipulator  than  closed  form  solution  techniques. 
Further,  most  numerical  iterative  techniques  utilised  in  "solving" 
manipulators  do  not  guarantee  the  finding  of  all  possible  solutions  that 
may  exist  for  the  manipulator.  Closed  form  methods  involve  a  solution 
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based  on  analytical  expressions  or  on  the  solution  of  a  polynomial  of 
degree  4  or  less,  such  that  non-iterative  calculations  suffice  to  arrive 
at  a  solution. 

Vithin  the  class  of  closed- form  solution  techniques,  two  major 
distinctions  can  be  made.  The  two  sub- classes  of  the  closed- form  method 
include  the  nnrelv  algebraic  solution  process  and  the  geometric  process. 
The  geometric  process,  however,  does  involve  a  degree  of  algebraic 
manipulation. 

A  recent  major  result  is  that  all  systems  with  revolute  and 
prismatic  joints  having  a  total  of  six  degrees  of  freedom  in  a  single 
series  chain  are  solvable,  at  least  numerically.  It  is,  however,  true 
that  it  is  only  in  special  cases  that  robots  with  six  degrees  of  freedom 
can  be  analytically  solved.  These  robots  possess  the  common 
characteristic  of  several  intersecting  joint  axes  and/or  many  (twist 

angle)  equal  to  0°  or  +90°.  A  sufficient  condition  that  a  manipulator 

with  six  revolute  joints  will  have  a  closed- form  solution  is  that  three 
neighbouring  joint  axes  intersect  at  a  point. 

A  well-known  solution  method  for  a  manipulator  with  all  six 
revolute  joints  and  with  three  axes  intersecting  at  a  point  is  the 
Pieoer^s  solution  process.  This  consists  of  transferring  the  known 
position  information  about  the  goal  point  to  the  point  of  intersection 
of  the  three  axes.  Successive  algebraic  manipulations  then  leads  to  a 
solution.  The  advantage  of  Pieper’s  technique  is  the  determination  of 
kinematic  singularities  during  the  solution  process,  as  well  as  the 
determination  of  all  possible  solutions  to  the  inverse  kinematics 
problem  for  the  manipulator  under  consideration. 

The  geometric  technique  of  closed- form  solutions  to  inverse 
kinematics  has  never  proven  to  be  popular,  due  to  its  inherent 
disadvantage  of  not  being  able  to  provide  kinematic  singularity 
information.  It  does  possess  the  advantage  of  providing  information 
about  the  determination  of  which  of  the  solutions  is  to  be  adopted. 
However,  the  technique  works  to  advantage  only  in  the  presence  of  "nice" 
twist  angles  like  0°  or  *90°  and  becomes  complicated  in  their  absence, 
and  often  even  when  some  of  the  are  "nice"  angles. 
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In  path  control  schemes,  it  is  often  necessary  to  solve  for  the 
inverse  kinematic  solutions  of  manipulator  arms  at  a  fairly  high  rate, 
sometimes  as  fast  as  20-30  Hz.,  or  faster.  As  such,  computational 
efficiency  is  often  an  issue  in  manipulator  inverse  kinematic  solution 
processes.  Numerically  iterative  processes  are  unable  to  fulfill  such 
requirements  and  are  therefore  not  generally  adopted,  unless  there  does 
not  exist  a  closed- form  solution  for  the  manipulator. 

The  structure  of  computation  is  also  of  importance.  It  is  more 
efficient  to  generate  all  of  the  joint  variables  in  parallel  and  to  use 
lookup  tables  than  to  generate  all  of  the  angles  serially.  It  is  also 
much  more  efficient  to  generate  only  one  solution  than  all  solutions, 
specially  when  all  of  them  are  not  required.  Another  time  saving 
proceedure  often  adopted  in  practice  is  the  generation  of  inverse 
kinematic  solutions  off-line,  storage  in  a  lookup  table  against  a  set  of 
goal  point  positions,  and  then  adjusting  the  solution  to  achieve  the 
exact  desired  goal  point  position.  The  remaining  orientation  joint 
variables  can  then  be  computed  by  using  the  closed- form  equations. 
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Figure  6.  The  lerlin  6500  Manipulator 
Frame  Assignments  -  Side  View. 


Figure  7.  The  Merlin  6500  Manipulator 
Frame  Assignments  -  Top  View. 
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V  THE  lERLIN  6500  KINEMATICS 


Frame  Assignments  for  the  Merlin  Manipulator 

The  first  step  involved  in  the  kinematic  analysis  of  manipulator 
mechanisms  is  the  setup  of  Cartesian  frames  at  each  joint  of  the 
manipulator.  This  is  done  following  the  rules  for  frame  assignment 
outlined  in  Chapter  V,  and  is  demonstrated  in  figures  6  and  7.  Frame 
assignments  have  been  performed  in  the  ’Home’  position,  defined  by  the 
inner  arm,  the  outer  arm  and  the  link  between  the  wrist  pin  and  the  face 
plate  being  parallel  to  the  floor  and  pointing  towards  the  front  of  the 
robot . 

The  origin  of  the  base  frame  {0}  has  been  located  at  the 
intersection  of  the  waist  and  shoulder  axes,  with  the  Zq  axis  aligned 

with  the  waist  axis.  This  location  of  the  origin  of  {0}  offers  the 
advantage  of  a  similarity  to  anthropomorphous  arm  geometry. 

The  origin  of  frame  {1}  coincides  with  the  origin  of  {0},  and  {1} 
is  coincident  to  {0}  at  the  ’home’  position.  The  origin  of  {2}  is 
located  at  the  center  of  the  inner  arm,  with  Tt^  positive  from  the  origin 

of  {2}  in  the  direction  formed  from  the  waist  to  the  shoulder. 

Frame  {3}  has  an  origin  located  at  the  center  of  the  outer  arm. 

lies  along  the  axis  of  joint  3  and  has  a  positive  direction  similar  to 
Z2,  measured  from  the  origin  of  {3}.  Zg  is  always  parallel  to  Z2. 

The  origins  of  {4},  {5}  and  {6}  are  located  at  the  center  of  the 

wrist  pin.  Z^,  Zg  and  Zg  lie  along  their  respective  axes,  with  Z^  and 

Zg  positive  towards  the  end  of  the  arm  and  Zg  positive  coming  out  of  the 

paper.  The  and  (i  =  1  to  6)  axes  are  set  up  according  to  the 

rules  defined  in  Chapter  4. 

The  Kinematic  Analysis  Procedure 

Following  the  assignment  of  frames  at  each  joint,  it  is  necessary 
to  determine  the  kinematic  parameters  for  the  Merlin  6500  arm.  These 
parameters  are  determined  by  using  the  rules  outlined  in  Chapter  4.  The 
direct  kinematic  analysis  of  the  mechanism  can  then  be  performed  by 
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foning  the  transfonsation  natrices  using  (4.6)  and  concatenating  them. 
There  is  always  a  unique  result  in  the  direct  kinematic  analysis  of 
robotic  arms. 


i 


immary  of  the  Kinematic  Parameters 
Since  all  the  joints  are  revolute ,  the  joint  variables  are 


1  to  6),  where  i  denotes  the  joint  number.  The  kinematic  parameters  are 
derived  using  the  Denavit- Hartenburg  convention,  defined  in  Chapter  V, 
and  are  summarized  in  Table  2. 


Table  2.  The  Denavit- Hartenburg  Parameters  for  the 
lerlin  6500  Left- Arm  Manipulator 


i 

“i-1 

(degrees) 

^i-1 

(  inches) 

*^2 

(inches) 

(degrees) 

Kinematic 

Range  (degrees) 

1 

0° 

0" 

0" 

h 

±  147° 

2 

-90° 

0" 

d2  (18.915”) 

>2 

+  56°  to  -  236° 

3 

0° 

a2(17.38") 

dj  (-6.915”) 

h 

+  56°  to  -  236° 

4 

-90° 

1 

0” 

d4  (17.24") 

U 

±  360°(continuous) 

5 

+90° 

0" 

0" 

h 

±  90° 

6 

1 

CD 

O 

o 

0" 

0" 

1 

±  360° (continuous) 

Note: 

1)  Right  hand  rule  used  (implying  counterclockwise  is  +  ve). 

2)  Source  :  Merlin  System  Operators  Guide  -  Version  3.0  /  June  1985. 


The  Direct  Kinematic  Solution  for  the  Left- Arm  Merlin 

The  general  forward  kinematic  task  is  to  compute  the  transformation 
matrix  relating  the  tip  of  the  end- effector  to  the  global  (or  world) 
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coordinate  frame  of  the  robot.  In  the  present  case,  ve  define  the 
direct  kinematic  problem  to  be  the  computation  of  closed  form  equations 
that  relate  the  position  of  the  origin  of  {6},  and  the  orientation  of 
the  last  link,  with  respect  to  {0}.  It  must  be  remembered  here  that  the 
global  frame  of  the  robot  is  at  a  height  of  46.45  inches  above  the  base 
and  that  the  hand  roll  frame,  {6},  is  located  at  the  wrist  pin. 

The  direct  kinematic  problem  thus  can  be  defined  as  the 
determination  of  the  gT  matrix,  computable  as 


The  transformation  matrices  in  (5.1)  are  given  by: 


(5.2) 


dg  =  18.915” 


(5.3) 


aj  =;  17.38” 

dg  2  -6.915”  (5.4) 
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According  to  the  principle  of  concatenation  of  transformations, 
developed  in  Chapter  3,  ve  have 


Therefore, 


®6 

0 


"6 

■®5®6 

0 


(5.8) 


(5.9) 


Further,  using  the  principle  of  transformation  matrix  concatenation,  ve 
have 
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(5.10) 


3m  3m  4m 
6^  ■  4^  •  6^ 


i.e. 


,T  = 


C4C5C6- S^Sg 

®5^6 

■  ®4‘^5‘^6'  ^4®6 
0 


■  ‘^4‘^5®6‘  ^4*^6 

-Ve 

®4^5®6‘  ^4‘^6 


-c 


4“5 

Ce 


«4«5 


0 1 
14 
0 


(5.11) 


Now,  since  the  joint  axes  for  {2}  k  {3}  are  always  parallel,  we  obtain 
gT  using  the  trigonometric  sum  of  angle  formulas 


*^23  ■  ‘^2‘^3  ■  ®2®3 
®23  ^  ®2^3  ^  ^2®3 


to  yield  a  simple  result. 


Since 


we  have 

^23  ■  ®23 

0  0 

■  ®23  ■  ^23 

0  0 

Now,  as 


0 

1 

0 

0 


^2^2 

■^2®2 

1 


(5.12) 


(5.13) 


(5.14) 
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ve  get 


where 

^*‘11  ■  '  ®23®5‘^6 

^^12  ~  ■^23t^4‘^5®6  *4*^6^  *  ®23®5®6 

^*■13  ^  ■  ^*^23^4*5  ■"  ®23^5^ 

^21  ”  ■  f^®4^5^6  *  ®4®6^ 

^*■22  ^  ®4‘^5®6  '  Ve 
^^23  ^  ®4®5 

^^31  "  '®23t^V5S  ■  ®4®6^  ‘  ^23 Ve 
^^32  ^  ®23^^4^5®e  ®4‘^e^  ^23®5®6 

^^33  ^  ®23®4®5  ■  ^23*^5 


^Px  "  ■‘^4®23  ■"  V2 
‘Py  =  ^2*  ^3 
^Pz  =  '^23  '  ^2®2 


(5.15) 


The  final  product  of  all  six  link  transformations  is  given  by 


«T  =  Jl  .  Jl 


(5.16) 


which  results  in  the  final 


0 

e 


T  matrix,  given  by 
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(5.17) 


^21 

-  r 
6^  ^31 

0 

where 


^11 

=  Cj[c23(c4CgCg 

-  8486) 

®23®5‘^6^ 

+  8j[s4C5Cg  + 

8485] 

^21 

=  Sj[c23(c4C5Cg 

-  8485) 

®23®5^6^ 

C48J] 

^31 

8485]  - 

^23®5‘^6 

^12 

=  c^[-C23(c4C5Sg 

+  S^Cg 

)  ®23®5®6^ 

®lt®4‘^5®6  ■ 

'485] 

^22 

=  ®lt‘‘^23(^4^5®6 

+  84Cg 

^  ®23®5®6^ 

+  Ci[V5®6  ■ 

‘=4'6] 

^32 

=  823[c^CgSg  +  s 

4"6]  " 

43®5®6 

'l3 

■  ■^l[‘^23^4®5 

®23‘^5^ 

-«l[»4®5^ 

^23 

=  -Sj [€230^85  + 

®23^5^ 

+  CjES^Sg] 

^33 

"  ®23^4®5  ■  ^23*^ 

5 

Px  = 

=  Cj[-d4S23  +  a2C 

2]  -  8j 

(d2  +  d3) 

Py  = 

=  8^[-d^S23  +  a2C 

2]  +  Cj^ 

(d2  +  d3) 

Pz  = 

■  ■‘*4*^23  ■  ^2®2 

^22  ^23  Py 

^^32  ^33  Pz 

0  0  1  . 


The  transformation  matrix  gX,  given  by  (5.17),  completely  defines 

and  locates  the  position  of  the  wrist  pin  and  orientation  of  the  link 
connecting  the  tip  of  the  Merlin  arm  to  the  wrist  pin,  with  respect  to 
the  base  frame.  The  position  of  the  tip  of  the  Merlin  manipulator  with 
respect  to  the  base  frame  is  easily  computable  from  the  above 
transformation  matrix.  This  requires  the  addition  of  the  product  of 

A 

each  term  in  the  ’approach’  vector  (the  third  column  vector  of  the  gT 


■atrix)  and  the  distance  between  the  tip  of  the  an  and  the  wrist  pin 
(or  the  distance  between  the  tip  of  the  an  and  the  point  under 
consideration),  to  the  corresponding  ten  in  the  position  vector  (fourth 
colunn  in  the  gT  natrix).  Thus,  if  *dg’  defines  the  distance  between 

the  tip  of  the  lerlin  6500  an  and  the  wrist  pin  (or  the  point  under 
consideration),  then  the  gT  matrix  can  be  modified  so  that  the  position 

data  provided  by  the  transformation  matrix  gT  refers  to  the  end  of  the 

an,  as  follows  : 

p;  =  =  Px  *  <*6  • 

Pj  =  ^(2.4)  =  P,  ♦  ig  • 

p;  =  =  Pz  -  .  “t(3,3) 

The  direct  kinematic  solution  could  have  alternatively  been 

perfoned  by  setting  the  origin  of  {6}  at  the  tip  of  the  lerlin  an  (or 
at  the  point  under  consideration),  instead  of  the  wrist  pin.  This 
method  would,  however,  cause  computational  complications  when  perfoning 
the  inverse  kinematic  solution  for  the  an,  since  the  solution  process 
by  Piepers  method  requires  three  axes  intersecting  at  a  point  and  the 
origin  of  the  three  frames  for  these  axes  are  set  at  the  point  of 
intersection. 

Direct  Kinematics  for  the  lieht-an  lerlin 

Ve  now  need  to  develop  the  direct  kinematics  for  the  right 
shouldered  lerlin  manipulator.  This  can  be  perfoned  either  by 
repeating  the  above  process  completely  for  the  right  arm  lerlin 
manipulator,  or  by  utilizing  the  solution  developed  for  the  left  arm 
manipulator  with  adjustments  being  made  to  the  values  of  the  kinematic 
parameters.  The  foner  process  involves  re- assigning  frames, 

detenining  the  Denavit-Hartenburg  parameters  and  then  computing  ®T  for 

the  right  an  manipulator.  The  latter  process  maintains  the  frame 
assignments  made  for  the  left  an  while  adjusting  the  numeric  values  of 
those  parameters  that  would  be  affected  by  the  conversion  of  the  left 
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ara  to  a  right  an  nanipulator,  and  using  the  direct  kinematic  equations 
for  the  left  an  robot,  given  by  (5.17). 

A  close  examination  of  the  left-  and  right-  shouldered  ans  reveals 
that  they  differ  kinematically  at  the  shoulder  only.  Ve  thus  adjust  the 
Denavit- Hartenburg  parameters  indicated  by  62  ^3  ^2  ~ 

inches  and  d^  ~  6.915  inches.  Equations  (5.17),  when  solved  for  with 

the  above  values  of  ^2  and  d^,  result  in  the  direct  kinematic  solution 

for  the  right  an  Merlin  manipulator. 


The  Inverse  Kinematic  Solution  for  the  Left- An  Merlin. 

Since  the  last  three  axes  of  the  Merlin  manipulator  intersect  at 
the  wrist  pin,  we  adopt  an  algebraic  method  (Pieper’s)  to  solve  for  the 
inverse  kinematic  solution.  Since  we  may  be  given  the  position  k 
orientation  of  the  hand- roll  plate,  and  the  origins  of  frames  {4},  {5} 
and  {6}  are  located  at  the  wrist  pin,  we  need  to  account  for  the 
distance  between  the  wrist-pin  and  the  tip  of  the  hand  roll  plate,  which 
is  2:  3.5”.  This  affects  the  position  vector  only  -  the  orientation 
vector  remains  unchanged. 

The  transformation  matrix  defining  the  position  and  orientation  is 
given  by  (5.17),  and  is 


;t  = 


11 

*^12 

^13 

Px 

21 

^22 

^23 

Py 

31 

^32 

^33 

Pz 

1 

0 

0 

1 

^12’ 

...  rgg. 

Px>  Py» 

Pj  are 

If  ^  X  ^  ^  « 

equations  given  in  (5.17). 

let  dg  be  the  distance  measured  from  the  tool  mounting  surface  to 

the  wrist  pin  (dg  3.5")  and  the  position  of  the  tool  mounting  surface 

T 

be  given  by  a  vector  p’  =  {p^  p*  p*}  ,  where 


p’  = 

r 

Pi 

Pi 

II 

r— - 

(X  o. 

.  dg . 

*^13 

^23 

Pi 

Pz 

1 - 

M 

L. _ 

Therefore,  the  position  of  the  wrist  pin  is  specified  by 


(5.19) 


Px 

Pi 

■®lC^23‘^4®5  ®23‘^5^  ‘  ®ll^®4®5^ 

Py 

= 

Pi 

-  -*6 

■®lt‘^23‘^4®5  *  ^23*^5^  ‘^lt®4®5^ 

Pz 

Pi 

j 

®23^4®5  ■  ^23*^5 

(5.20) 


Ve  now  examine  the  kinematic  parameters  to  determine  the  number  of 
solutions  that  will  be  obtained  when  solving  the  inverse  kinematics  of 
the  lerlin  robot. 

Since  a^  =  a^  =  a^  =  0,  we  determine  (from  Chapter  4)  that  the 

number  of  solutions  for  the  left  shouldered  Merlin  arm  will  be  four  in 
number.  Since  a^,  a^  and  a^  are  unaffected  by  the  shoulders 

configuration,  four  further  solutions  will  be  obtained  for  the  right  arm 
lerlin  robot.  This  results  in  a  total  set  of  eight  solutions  for  the 
inverse  kinematics  of  the  lerlin  robot.  These  solutions  can  be  seen  to 
include  the  following  configurations  for  each  of  the  left  and  right 
arms  : 

1)  Inner  arm  up,  outer  arm  down,  wrist  roll,  wrist  pitch. 

2)  Inner  arm  down,  outer  arm  up,  wrist  roll,  wrist  pitch. 

3)  Inner  arm  up,  outer  arm  down,  wrist  ’flipped’  over. 

4)  Inner  arm  down,  outer  arm  up,  wrist  ’flipped’  over. 

Four  similar  solutions  exist  for  the  lerlin  right- shouldered 
manipulator. 

Ve  now  proceed  to  solve  the  inverse  kinematics  of  the  lerlin  left 
arm  manipulator.  The  inverse  kinematic  solution  process  requires 
solving 
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“t  =  ®T(»j)  .  (5  21) 

for  i  =  1  to  6,  when  gT  is  given  as  numeric  values,  with  the 

position  vector  of  gT  having  been  adjusted  according  to  (5.20),  if 
necessary. 

Putting  the  dependence  of  6^  on  the  left  side  of  the  equation  gives 

[?T(«l)]  .  =  1t(()2)  .  =T(»3)  .  3i(K^).  (5.22) 

Inverting  ,  we  rewrite  (5.22)  as 


Cj^  Sj^  0  0 

Sj  Cj  0  0 

0  0  10 

0  0  0  1 


^11 

^12 

^13 

^21 

^22 

^23 

h  - 

^31 

^32 

*^33 

Pz 

0 

0 

0 

1 

(5.23) 


where  gT  is  given  by  (5.15). 

Equating  the  (2,4)  elements  from  both  sides  of  (5.23),  we  get 
■«lPx  ^  "^iPy  =  *^2  ^  ^3  (5-24) 

Substituting 


p^  =  p  Cos  ^  and 

Py  =  p  Sin  ^ 


where  p  =  +  Py^ 

and  ^  =  itan2  (Py,  p^) 


into  equation  (5.24),  we  get 


(5.25) 


(5.26) 
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p{Sin4  Costf^  -  Cos^  Sin$^)  =  dj  +  dg 


which  results  in 


Sin(^  -  = 


dj  *  dj 


(5.27) 


Using 


Sin^  A  +  Cos^  A  =  1, 


we  get 


rd^~+~d^ 


Co8(^  -  =  ± 


Since  we  know  Sin(^  -  and  Co8(^  -  9^)y  we  find  -  $^)  as 


(5.28) 


(^  -  0^  =  Atan2 


^2  *  ^3 


<*2  *‘*3 


Using  the  value  of  p  from  (5.26),  we  get 


(^  -  ^j)  =  Atan2 


[dj.d,], 


*^2  ^3 


(5.29) 


i.e. 


0j  =  Atan2(Py,  p^)  -  Atan2 


K  "  <*3 


J. 


^2  *  *^3 


(5.30) 


In  equation  (5.30),  we  have  utilized  the  Atan2  function  to 
determine  the  value  of  6^.  Use  of  the  cosine  or  arc  sine  function  would 

lead  to  inaccurate,  inconsistent  and  ill-conditioned  solutions,  since 
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the  accuracy  of  the  arc  cosine  function  in  determining  the  angle  is 
dependant  on  the  angle  [Cos  $  -  Cos(-^)],  while,  when  Sin  0  approaches 
zero,  ^  =  0°  or  +  180°.  A  more  consistent  approach  is  to  use  the  Atan2 

function,  which  returns  the  value  of  6  adjusted  to  the  proper  quadrant. 


The  Atan2  function 

is 

defined 

as 

follows 

0°  < 

0 

< 

90° 

for 

+  X 

and  +  y 

0  =  Atan2(y,  x)  = 

90°  < 

0 

< 

180° 

for 

-  X 

and  +  y 

- 

180°  < 

0 

< 

-  90° 

for 

-  X 

and  -  y 

- 

90°  < 

0 

< 

0° 

for 

+  X 

and  -  y 

Note  that,  in  (5.30),  there  are  two  possible  solutions  to  6^, 

depending  on  the  ±  sign  in  the  second  term  of  the  equation.  The 
positive  solution  is  obtained  for  the  left  arm  lerlin  6500  manipulator, 
while  the  negative  solution  represents  the  inverse  kinematic  solution 
for  0^  for  the  right  arm  Kerlin  6500  manipulator  with  different  frame 

assignments  than  those  made  for  the  left  arm. 

Since  0^  is  now  known,  we  now  know  the  left  side  of  equation  (5.22) 

and  (5.23). 

Equating  the  (1,4)  elements  of  (5.23),  we  have 

CjP,  ♦  SjP,  =  -d^S^g  *  agCj  (5.31) 

Equating  the  (3,4)  elements  of  equation  (5.23),  we  have 

Pz  =  ■‘‘4'23  ■  ‘^‘2  (5-32) 

Squaring  equations  (5.24),  (5.31)  and  (5.32)  and  adding,  we  get 
Px  +  Py  +  Pz  =  a2  ^  d4  -  2a2d4S3  +  [d2  +  dgj^ 
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Therefore, 


-202(1483  =  Px  +  Py  +  P2  -  4  ■  K  ‘^3]^  ■  4 

which  results  in 


83  =  ■  J 

2  2  2  2  f 

Pt  Pv  P®  ■  ^2  ' 

'*2  •*  '*3]^  -'*4 

X  y  z  z 

A  OJ  1 

Since 

2  2  , 
®3  ^  ^3  '  ^ 

we  have 


c 


3 


s 


2 

3 


Therefore 


9^  -  Atan2 


!•  ‘J 


So,  ±  1  -  si 


where 


-1 


282(14 


2  2  2  2 
Px  ^  Py  ^  Pz  •  ^2 


-  [<*2  *  <*3]'  -  >*4 


(5.34) 


(5.35) 


Thus,  ^3  can  have  two  values,  depending  on  the  *  sign  used  in 

(5.34).  Each  of  the  solutions  represents  the  elbow  up  or  down  solution. 
Both  of  the  above  solutions  for  9^  are  valid  for  the  left  and  right  arm 

Berlin  robot.  The  values  of  d2  and  d3  that  are  used  in  (5.35)  will 
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depend  on  whether  the  arm  solution  desired  is  for  the  left  or  for  the 
right  arm. 

Equation  (5.22)  can  now  be  written  so  that  we  have  the  left  side  as 
a  function  of  the  known  variables  0^  and  0^  and  the  unknown 


-1 


(5.36) 


Since 


we  have 


Ve  invert 


^1^23 

■^1®23 

-«1 

®1^23 

'®1®23 

■®23 

■^23 

0 

■^2®2 

0 

0 

0 

1 

using  (3.18), 

to  get 

(5.37) 


^^1^23 

®1^23 

■®23 

‘^2^3 

1 

■®1®23 

■‘^23 

^2®3 

-^1 

0 

- (d2+d3) 

0 

0 

0 

1 

(5.38) 


Equation  (5.36)  can  now  be  written  as 
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*^1^23 

®1^23  ■ ®23 

■®2®3 

■*^1*23  ■ 

®1®23  ^^23 

®2®3 

-«1 

0 

-(dj^dg) 

0 

0  0 

1 

where 

’ 

®4^5‘^6‘  ®4®6 

-C4C5S 

-SgS 

3t  _ 

■  ®4^5^6’  *^4^6 

S4C5S 

0  0 


*^11 

*■12 

^13 

Px 

'21 

**22 

*^23 

Py 

s  ^t 

(5.39) 

^31 

^32 

*■33 

P2 

0 

0 

0 

1. 

®4‘^6 

. 

<^4*5 

0 

] 

^5 

^4*^6  ®4®5  ® 

0  1 


Equating  the  (1,4)  and  (2,4)  elements  of  equation  (5.39),  we  have 


^1^23Px  ®l‘^23Py  '  ^23^2  '  ^2^3  ""  ® 

■‘^l®23Px  ■  ®l®23Py  ‘  *^23^2  ^2®3  ""  ^4 


(5.40) 


Taking  the  known  terms  to  the  right  side  of  the  equation,  we  have 


‘^23(^lPx  ^  ®lPy^  '  *23^2  "  ^2^3 
®23(^lPx  ®lPy^  ^23^2  ^  ^2®3  '  *^4 


(5.41) 


Let 


A  =  Cjp^  +  SjPy 
B  =  p 

^  “  ^2^3 
®  “  ^2®3  ’  *^4 

Therefore,  (5.41)  now  becomes 
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(5.42) 


*^23^  ■  ®23®  "  ^ 

^23®  ^  ®23^  ■  ® 

Solving  for  C23  and  S23  by  Cramers  rule,  we  have 

.  (^2^3  ■  ‘^4)Pz  ^  ^2‘'3(^lPx  ^  SfPy) 

*^23  =  - 2 - 2 - 

Pz  +  (ClP^  -  s^Py) 

(^2®3  ■  ®lPy^  ■  ^2‘^3Pz 

Pz  "  (‘^iPx  ^  ®lPy)^ 

and 

^23  =  Atan2|^{(a2S3  -  d4)(cjp^  +  s^Py)  -  a2C3Pjg}, 

{(^2®3  ■  ^4)Pz  ■"  ^2*^3 (‘^iPx  ®lPy)] 

Due  to  the  four  possible  combination  of  solutions  of  0^  and  ^3, 
there  will  be  a  total  of  four  possible  solutions  for  ^23*  is  such,  the 
four  possible  solutions  for  $2  are  computed  as 

^2  “  ^23  ‘  ^3  (5.46) 

where  the  appropriate  solution  for  0^  ’^®®d  when  forming  the 

difference.  Since  the  computed  value  of  0^  is  used  in  solving  (5.45), 
and  hence  (5.46),  the  left  arm  (positive)  solution  for  provides  the 
left  arm  solution  for  $2  (which  account  for  two  of  the  four  solutions 
obtained  above),  while  the  right  arm  solution  for  0^  (the  negative 
solution)  provides  that  solution  for  02  which  is  valid  for  the  right  arm 
only  (and  which  account  for  the  remaining  two  solutions  obtained 


(5.43) 

(5.44) 
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for  $2)- 

Ve  nov  knov  the  entire  left  side  of  equation  (5.39).  Equating  the 
(1,3)  and  (3,3)  elements  froa  both  sides  of  (5.39),  ve  get 

’^13*^1^23  ^  ^23®1^23  ‘  '33®23  ^  ‘^4*5 

(5.47) 

-^13®1  ^  Vl  =  ®4«5 

If,  in  (5.47),  Sg  #  0,  ve  solve  for  as  follovs 

H  =  ('**13®!  ^  "s" 

and 

^^4  ""  ('*^13‘^l‘^23  '  *^23®i43  *^33^23^ 

Therefore 

h  ■  Atan2j(-rjgSj  +  r23^1^’  (‘*’13^1^23  ‘  *^23^1*^23  ^33®23^]  (5-48) 

If,  hovever,  Sg  =  0,  then  ^g  =  0°  or  180®,  and  the  aanipulator  is 

in  a  singular  configuration,  in  vhich  the  vrist  roll  and  hand  roll  axes 
(z^  and  Zg)  line  up  and  cause  the  same  motion  of  the  last  link  of  the 

robot.  In  such  a  case,  all  that  can  be  solved  for  is  the  sum  or 
difference  of  9^  and  This  condition  of  singularity  is  detected  by 

checking  to  see  if  the  tvo  arguments  of  the  itan2  function  of  equation 
(5.48)  are  close  to  zero.  If  they  are,  9^  should  be  chosen  to  be  the 

present  value  (or  any  other  arbitrary  value)  of  the  vrist  roll  angle. 
Vhen  9q  is  computed  at  the  last  stage  using  the  present  (or  arbitrary) 

value  of  9^y  it  is  adjusted  according  to  the  value  chosen  for  9^. 
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Considering  equation  (5.21)  again,  we  now  know  ^2*  ^3  ^4- 

So  we  rewrite  equation  (5.21)  to  get  all  the  knowns  on  the  left  side,  as 
follows: 


Ve  know  that 


(5.49) 


5t  =  2t  . 

4  3  4 


Since 


(iB)-l  =  (B)-‘  .  (A)-‘ 


we  have 


M-i  .  f3Tl-l  ,  'M-l 


3ml"  1 


Ve  compute  using  (3.18)  and  (5.5)  as 


0 

0 


0 

1 


-^4 

0 

0 


0 

0 

-d, 


Having  computed  in  (5.38),  we  solve 


JtI-1 


ItI-i 


«t1-‘ 


(5.50) 


to  get 
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®l‘^23V®l®4 

®lV4'‘^l®4 

■  ®23®4 

■  ‘^1^23®4‘^®i‘^4 

■®l‘^23®4’‘^1^4 

®23®4 

■‘^1®23 

■®1®23 

■‘^23 

0 

0 

0 

1 


Equation  (5.49)  can  now  be  written  as 


where  gT  is  given  by  (5.17)  and  ^  by  (5.9) 

Equating  the  (1,  3)  and  (3,  3)  terms  of  (5.52),  we  have 

-ri3(cjC23C4  +  s^s^)  -  *“33^23*^4  ‘  ®5 

■^l®23*'l3  ■  ®1®23*‘23  ‘  ®23*‘33  ^5  * 


Ve  therefore  solve  for  0^  as 


(5.51) 


(5.52) 


(5.53) 


(5.54) 


^5  ~  Atan2(Sg,  Cg) 
where  Sg  and  Cg  are  given  by  (5.53). 

Rewriting  (5.21)  to  get  the  known  terms  on  the  left  side,  we  have 

.  »T  =  (5,55) 

As  before 


4, 
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Therefore 


(5.56) 


Ve  compute 


j^gTj'^  using  (3.18)  and  (5.6)  as 


0  ■ 

0 

0 

1  i 


Having  computed 


Jt]'^  in  (5.51), 


we  solve  for 


as 


- S5(c^C23C4+SjS4)- CjS23C5 

^^1^23^4  '  ®l‘^4 
0 


‘^5(®l‘^23V  ‘^1®4^‘  ®1®23®5 

■  ®5^®l‘^23‘^4‘  ‘^1^4^'  ®1®23‘^5 
®l‘^23®4  ‘^l‘^4 

0 


®23^4V  ^23  ®5 

®23^4®5‘ ^23^5 
■ ®23®4 
0 


{c5(-a2C3C4+S4(d2+d3)+S5(a283-d4)} 

{■  ®5(‘  ^2®3^4'^®4(‘^2'^‘^3^'^‘^5^®’2®3’  ^4^  ^ 
"^2^3®4  ’  ®4^‘^2'^^3^ 

1 


(5.57) 


Since  we  know  j^gTj  from  (5.7),  we  equate  the  (1,1)  and  the  (3,1) 
elements  of  (5.55),  to  get 


{05(0102304  +  s^s^)  -  CiS2385|rj^j  + 

{05(8102304  -  O1S4)  -  SiS23S5|r2i  -  {S23C4O5  +  C23S5}r3i  =  Og 

-  {01O2384  -  8iC4}rii  -  {S1O2384  +  Oi04}r2i  +  {s23S4}r3i  =  Sg 


(5.58) 
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Ve  therefore  deteraine  0q  by 

=  itan2(8g,  Cg)  (5.59) 

where  Sg  and  Cg  are  given  by  (5.58). 

Since  there  are  two  possible  solutions  for  each  of  9^  and 
equation  (5.59)  results  in  a  total  of  four  solutions  for  9^.  If  the 
positive  value  for  9^  is  used  in  solving  (5.59),  then  the  solution 
obtained  for  9^  by  (5.59)  is  for  the  left  arm  lerlin  robot,  while  if  the 
negative  solution  for  9^  is  used,  we  obtain  the  solution  for  the  right 
am  lerlin  robot. 

Additional  solutions  are  obtained  by  flipping  over  the  wrist  of  the 
manipulator,  and  are  given  by 

91=  9^*  1801  , 

9^  =  -9^  and 

9^q  =  9q  +  1801. 

After  all  of  the  above  eight  solutions  have  been  computed,  some  (or 
all)  of  them  may  ha.e  to  be  discarded  because  of  joint  limit  violations. 
Of  the  remaining  valid  solutions,  it  is  advisable  in  most  cases  to 
choose  the  one  closest  to  the  current  configuration  of  the  manipulator. 

Inverse  Kinematics  for  the  Right- am  lerlin  Manipulator 

Of  the  above  eight  solutions  which  constitute  the  solution  set  for 
the  inverse  kinematics  for  the  left  and  right  am  lerlin  6500 
manipulator,  those  solutions  obtained  using  the  positive  solution  for  9^ 

represent  the  inverse  kinematic  solution  for  the  left  am  lerlin  robot, 
while  the  four  solutions  obtained  using  the  negative  solution  for  9^ 

represent  the  solution  set  for  the  inverse  kinematics  for  the  right  am 


(5.60) 
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Merlin  robot  with  adjusted  frames. 

An  alternative  procedure  for  developing  the  closed- form  inverse 
kinematics  for  the  right  arm  Merlin  manipulator  involves  using  the  same 
set  of  equations  developed  for  the  left  arm  Merlin  but  adjusting  the 
kinematic  parameters  involved  in  converting  from  a  left  arm  Merlin  robot 
into  a  right  arm  Merlin.  As  explained  in  Chapter  5,  this  essentially 
involves  retaining  the  orientation  of  the  frames  assigned  to  the 
individual  joints  and  negating  those  parameters  that  will  effect  the 
conversion  of  the  left  arm  to  a  right  arm  Merlin  robot,  viz.  dg  and  dg. 

Thus,  setting  -  -18.915"  and  dg  ~  6.915"  will  result  in  a  left  arm 

Merlin  becoming  a  right  arm  Merlin  robot.  Using  the  above  adjusted 
parameter  values  for  the  right  arm  parameters  in  the  left  arm  {9^ 

positive  solution  set)  equations  results  in  the  solution  being  obtained 
for  the  right  arm  Merlin  6500  manipulator.  The  major  advantage  of  this 
process  is  the  avoidance  of  new  frame  assignments,  reduced  code  in 
computer  implementations  as  well  as  the  fact  that  new  frame  assignments 
and  joint  angle  measurement  processes  do  not  have  to  be  followed.  This 
process  has  therefore  been  adopted  in  the  computer  implementation  of  the 
equations  (Appendix  IV). 
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VI  VOIKSPACE  DEVELOPMENT  FOE  THE  MERLIN  6500  ROBOT  ARM 


Vorkspaces  of  Manipulators 

The  working  voliine  of  a  isanipulator  is  called  the  nanipulators 
workspace.  The  workspace  of  any  aanipulator  is  defined  as  the  set  of 
positions  that  the  end- effector  can  achieve  when  the  joints  vary  over 

5 

the  full  range  of  possible  values  . 

Manipulator  workspaces  are  divided  into  reachable  and  dexterous 
workspaces.  The  reachable  workspace  for  a  manipulator  arm  is  defined  as 
that  volume  of  space  that  the  manipulator’s  end- effector  can  reach  in  at 
least  one  orientation  (Appendix  Al.lO).  The  dexterous  workspace  of  a 
manipulator  is  defined  as  that  volume  of  space  which  the  end- effector 
can  reach  with  all  possible  orientations  (Appendix  Al.ll).  The 
dexterous  workspace  of  a  manipulator  is  always  a  sub- set  of  the 
reachable  workspace. 

Each  of  the  dexterous  and  reachable  workspaces  of  a  manipulator 
possess  an  outer  and  inner  boundary.  The  outer  boundaries  are 
determinable  by  the  set  of  positions  in  Cartesian  space  corresponding  to 
the  tip  of  the  end- effector  (or  tool)  of  the  manipulator,  when  the 
joints  of  the  manipulator  are  taken  through  their  full  range  of  motion. 
The  inner  boundary  for  manipulators  (with  the  last  three  axes 
intersecting  at  a  common  point)  are  defined  by  the  set  of  positions  in 
Cartesian  space  that  the  point  of  intersection  goes  through  when  the 
joint  motions  are  taken  through  their  full  range  of  motion. 

Previous  work  on  manipulator  workspace  generation  has  been 
performed  by  a  variety  of  methods® Since  manipulator  workspaces 
are  geometrically  complex,  it  has  been  found  easier  to  obtain  an 
understanding  of  their  shape  by  developing  their  workspaces  in  two 
dimensions  i.e.  in  planes.  The  complete  workspace  is  a  composite  of  the 
two  dimensional  workspaces  in  all  three  dimensions  and  can  be  formed  by 
overlaying  the  two  perpendicularly,  and  aligning  the  axes  common  to  both 
planes.  The  workspaces  that  are  commonly  developed  for  manipulators 
consist  of  the  horizontal  workspace  (a  projection  of  the  manipulator 
workspace  onto  a  horizontal  plane)  and  the  vertical  workspace  (a 
projection  of  the  workspace  onto  a  vertical  plane). 

The  process  of  two  dimensional  workspace  development  consists  of 
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the  division  of  the  degrees- of- freedom  of  the  manipulator  into  those 
that  act  in  the  vertical  plane,  those  that  act  in  a  horizontal  plane  and 
those  successive  degrees- of- freedom  that  can  be  combined  together  to 
produce  a  motion  with  an  axis  perpendicular  to  the  axes  in  which  each 
joint  acts  separately. 


The  only  possible  link  motions  in  the  horizontal  plane  that  the 
Merlin  6500  arm  possesses  consist  of  the  waist  rotation  over  a  range  of 
294  degrees  and  the  wrist  yaw  over  a  range  of  180  degrees.  The  wrist 
yawing  motion  is  formed  by  a  wrist  roll  of  90  degrees,  followed  by  a 
wrist  pitching  motion  over  the  full  range  of  180  degrees.  The 
manipulator  is  therefore  kinematically  representable  as  shown  in 


figure  8. 


Figure  8.  The  Horizontal  Plane  Representation 
of  the  Merlin  6500  arm 
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The  lerlin  6500  horizontal  degrees- of- freedom  can  be  assumed  to 
form  a  manipulator  with  the  kinematic  parameters  as  in  Table  3. 


Table  3.  The  lerlin  6500  Horizontal  Plane  Representation 
and  the  Corresponding  Denavit- Hartenburg  Parameters. 


i 

o 

“i-1 

It 

^i-1 

M 

"i 

and  range 

A 

0“ 

0 

0 

+  147° 

B 

0“ 

-d  (  0!  12") 

0 

90°  constant 

C 

0® 

1"  (=:  34.62") 

0" 

0  to  - 180° 

Using  these  kinematic  parameters,  the  transformation  matrices  that 
relate  each  frame  with  respect  to  the  previous  frame  can  be  computed 
using  the  general  form  of  the  i  matrix,  given  in  equation  (4.6).  It 

is  required  here  to  determine  both  the  end-points  of  each  link.  These 
end-points  can  be  determined  from  the  last  column,  i.e.  from  the 
position  vector,  of  the  concatenated  transformation  matrices.  Since  the 
intersection  of  the  waist  and  shoulder  axes  is  always  at  a  fixed  point, 
the  graphical  origin  is  located  at  this  point.  The  process  of  transform 
concatenation  can  be  performed  so  that  the  determination  of  the 
end-points  is  made  during  the  concatenation  process. 

Using  the  general  form  of  the  transformation  matrix  developed  in 
(4.6),  we  develop  the  kinematic  transform  matrices  relating  each 
link-joint  system  to  its  previous  one.  Thus, 


Cj  -Sj  0  0 


0  0  10 
0  0  0  1 
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0  -d  ■ 

0  0 

1  0 

0  1 


Since 


A 

B 


T 


we  have, 


0 

B 


T 


12 

■®12 

0 

-dCj  ' 

12 

^12 

0 

-ds^ 

0 

1 

0 

0 

0 

1 

Further,  since 


B 

C 


T 


we  have 


(6.2) 


(6.3) 


(6.4) 


(6.6) 
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(6.6) 


^^123 

■®123 

0 

(-dCj  + 

®123 

*^123 

0 

(-dSj  +  Ls^2) 

0 

0 

1 

0 

0 

0 

0 

1 

From  the  position  vector  (last  column)  of  equations  (6.4)  and 
(6.6),  we  can  extract  the  graphical  co-ordinates  of  the  end-points  of 
the  lines  representing  the  kinematic  skeleton  of  the  lerlin  6500 
manipulator  arm. 

Ve  finally  need  to  compute  the  position  of  D  with  respect  to  the 
origin  A.  From  figure  8,  we  note  that  {D}  is  located  on  the  sliding 
vector  (Y-axis)  of  {C},  at  a  distance  dg”  from  {C}.  The  position  of  {D} 

in  each  of  the  X-  and  Y-  coordinate  directions  is  given  by 


Dx  -  +  d^  .  ('8.,.j^) 


=  Cy  +  d«  .  (c 


*6 


123^ 

123) 


(6.7) 


The  graphical  coordinates  of  each  point  A,  B  and  C  are  therefore 
determinable  from  (6.4),  (6.6)  and  (6.7),  and  are 


B  = 

X 

C  = 

X 

D  = 


0  ; 

-dCj  ; 

-dCj  ^^12  ^ 

-dCj  +  1-^12  ■  *^6^123  ’ 


A,  =  0 

By  =  -dSj 

Cy  =  -dSj  +  lSj2 

D  =  -dSj  +  l5j2  +  djCj23 


(6.8) 


where  ’s’  denotes  the  sine,  and  ’c’  the  cosine,  of  the  sum  of  the 
joint  angles  in  the  subscript. 


The  Vertical  Workspace  of  the  Merlin  6500  Manipulator 

Ve  now  proceed  to  develop  the  vertical  plane  workspace  of  the 
lerlin  6500  left  arm  manipulator. 
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17.38 
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Figure  9.  The  Vertical  Plane  Eepresentation 
of  the  Merlin  6500  arm 

In  the  vertical  plane,  the  Merlin  6500  arm  can  be  represented  by 
its  degrees- of- freedom  which  allow  motion  only  in  that  plane.  These 
degrees- of- freedom  are  the  Shoulder  Pitch,  Elbow  Pitch  and  Wrist  Pitch. 
Thus,  with  the  base  frame  origin  setup  according  to  the  graphical  X,  Y 
and  Z  coordinate  system,  we  are  able  to  represent  the  manipulator  arm  as 
shown  in  figure  9. 

This  system  possesses  the  following  Denavit-Hartenburg  parameters 
(summarized  in  Table  4) 
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Table  4.  The  Merlin  6500  Vertical  Plane  Eepresentation 
and  the  Corresponding  Denavit-Iartenburg  Parameters. 


i 

®i-l 

^-1 

"i 

9^  and  range 

2 

0 

II 

0 

0 

ao  =  0" 

d2  =  0” 

+  236°  to  -56° 

3 

«2  = 

ag  =  17.38” 

d3  =  0” 

+  146°  to  - 146° 

5 

04  =  0° 

a^  =  17.24” 

<*5  =  O" 

_ 

+  90°  to  -  90° 

Now,  using  the  general  form  of  ^^^T,  given  by  (4.6),  we  have 


■®2  ^ 

0  1 

0  0 


0 

0 

0 

1 


(6.9) 


C3  -  S3  0 


2 

3 


T 


0 


0 


C3  0 

0  1 

0  0 


^2 

0 

0 

1 


(6.10) 


and 


"5 


0 

0 


0 

0 


0 

0  0 

1  0 
0  1 


(6.11) 
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Using  the  principle  of  transformation  matrix  concatenation,  we  have 


which  results  in 

f  *^23  '^23  ®  ^2*^2 


0 

3 


T 


®22 

0 

0 


*^23 

0 

0 


0 

1 

0 


(6.12) 


(6.13) 


Ve  extract  the  I-  and  Y-  coordinate  positions  of  {3}  from  the 
position  vector  of  (6.13)  to  get 


®x  ■  ^2  ^2 
By  =  a2  Sj 


and 


Further,  since  we  have 


we  get 


'235 

®235 

0 

^2^2  ^4*^23 

‘235 

^235 

0 

*^2®2  ^  ^4®23 

0 

0 

1 

0 

0 

0 

0 

1 

(6.14) 


(6.15) 


(6.16) 


From  (6.16),  we  extract  the  position  of  C,  given  by 
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"  ^2^2  *  ^4^23 
^  ^2®2  ^4®23 


(6.17) 


Since  {D}  is  dg"  away  from  {5},  along  the  normal  vector  (X5) »  the 
position  of  D  can  be  computed  as 


®x  ■  ^6-^235 

%  "  S  ^  ‘^6'®235 


®x  ■  ^2‘^2  ^4‘^23  ‘*6^^235 
°y  "  ^2®2  ^4®23  ‘^6^235 


Since  we  now  know  all  the  endpoint  positions,  we  can  develop  the 
workspace  of  the  manipulator  in  two  planes  (the  horizontal  and  the 
vertical),  given  the  range  of  motion  of  each  joint.  A  computer 
simulation  of  each  of  the  planar  workspaces  of  the  Merlin  6500 
manipulator  workspace  was  performed  and  the  results  are  shown  in  figures 
10  and  11.  The  source  code  listings  used  to  generate  the  vertical  and 
horizontal  plane  workspaces  (using  Fortran  and  the  DISSPLi  graphics 
package)  are  given  in  Appendix  V. 
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SHOULDER  STEP  SIZE  »  9.125  DEGREES 
ELBON  STEP  SIZE  -  9.125  DEGREES 
WRIST  PITCH  STEP  SIZE  »  lO.D  DEGREES 


Figure  10.  Computer  Simulation  Results  of  the 
Merlin  6500  Vertical  Plane  Workspace. 
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WRIST  STEP  SIZE  =  1.0  DEGREES 
WRIST  YflW  STEP  SIZE  =  5.0  DEGREES 


Figure  11.  Computer  Simulation  Results  of  the 
Merlin  6500  Horizontal  Plane  Workspace. 


65 


VII  THE  UTAH/MIT  DEXTEROUS  HAND 


Previous  work 

The  kinematics  of  articulated  hands  has  been  examined  in  detail 
12 

previously  ,  while  that  of  the  Dtah/MIT  dexterous  hand  has  been  solved 
by  Narasimhan  .  The  present  work  differs  from  that  performed  at  MIT  in 
that  the  frame  assignments  have  been  performed  with  a  base  frame  set  up 
at  the  intersection  of  the  0^^  joint  axis  for  the  middle  finger  and  the 
thumb.  Further,  the  origin  of  the  frames  for  joint  0  of  each  finger  has 
been  located  at  the  intersection  of  the  axis  of  joint  0  with  the  a^ 

perpendicular  to  axis  0  and  which  passes  through  joint  1.  The  process 
of  direct  kinematic  closed- form  equation  development  for  the  dexterous 
left-  and  right-  hand  has  also  been  presented  in  detail.  The  current 
work  has  been  performed  keeping  in  mind  the  fact  that  the  Dtah/MIT 
dexterous  hands  have  to  be  attached  to  manipulators  for  dexterous 
tele- operation  purposes.  Further,  the  current  work  proceeds  to  examine 
the  differences  in  the  direct  kinematics  of  the  left-  and  right- 
dexterous  hands  and  proposes  a  minimal- change  method  for  solving  the 
direct  kinematics  of  the  right  hand,  using  the  closed- form  equations  of 
the  left  hand.  The  major  advantage  of  such  a  method  is  that  the  direct 
kinematics  of  one  dexterous  hand  only  has  to  be  programmed,  since 
changes  to  the  values  of  the  appropriate  Denavit-Hartenburg  parameter 
values  will  allow  for  switching  from  the  left  to  the  right  hand,  and 
vice- versa. 

Direct  Kinematics 

The  generalized  process  of  direct  kinematic  closed- form  equation 
development  has  been  dealt  with  in  Chapters  4  and  5.  The  frame 
assignments  for  the  left- fingered  Utah/MIT  dexterous  hand  is  as  shown  in 
figures  12  and  13,  and  follows  the  basic  frame  assignment  procedure 
outlined  in  Chapter  4. 

The  direct  kinematic  analysis  of  the  Utah/MIT  dexterous  hand 
involves  the  division  of  the  hand  into  four  manipulators,  viz.  the 
thumb,  finger  1,  finger  2  and  finger  3.  Finger  1  denotes  the  finger 
situated  on  the  thumb  side  of  the  palm,  finger  2  the  middle  finger  and 


66 


Figure  12.  The  Utah/llT  Dexterous  lands 
-  Top  View 
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Figure  13.  The  Utah/MIT  Dexterous  Hands 
-  Side  View 
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4.25 


finger  3  the  finger  situated  on  the  opposite  side  of  the  pals  froa  the 
thumb.  Since  the  fingers  1,  2  h  3  are  kinematically  similar  with 
respect  to  the  base  frame  {0}»  except  for  the  offset  along  1^,  i.e.  a^, 

we  will  let  a^  be  a  variable  depending  on  the  finger  we  are  referring 

to. 

Further,  since  we  are  dealing  with  a  multiple  manipulator  system,  a 
trailing  sub- script  ’t’  will  be  used  to  refer  to  the  thumb  while  ’f’ 
(f  =  1,  2  or  3)  will  be  used  to  refer  to  the  fingers  1,  2  or  3. 

Thumb  Kinematics 

The  direct  kinematic  closed- form  equations  for  the  thumb  are  now 
developed  with  respect  to  the  common  base  frame. 

Denavit- lartenburg  Parameters  for  the  Thumb 

Ve  now  develop  the  Denavit- Hart enburg  parameters  for  the  thumb, 
following  the  rules  developed  in  Chapter  4.  The  joint  variables  for 
each  joint  are  the  joint  angles,  (i  =  1  to  4).  Ve  have  again  adopted 

the  right-hand  rule  (counter-clockwise  is  positive)  for  determining  the 
sign  of  the  angles. 

Table  5.  The  Denavit- lartenburg  Parameters  for  the 
Thumb  of  the  Dtah/VIT  Hand. 


Thumb  Transformation  Matrices: 

The  thumb  transformation  matrices  are  developed  using  the  ^^^T 
matrix  developed  in  Chapter  4  and  is  given  by  equation  (4.6). 


c.  -s.  0  a^ 

11  0 

s^  Cj  0  0 

0  0  1  dj 

0  0  0  1 


Cj  -  S2  0  a^ 

0  0-10 

^2  ^  ® 

0  0  0  1 


0 

0 

1 

0 


^2 

0 

0 

1 


Since  =  0°  (7.1) 


Since  Oj  =  90°  (7.2) 

and  d2  =  0" 


Since  =  0°  (7.3) 

and  dg  =  0" 


Since  Oj  =  0°  (7.4) 

and  d^  =  0” 


Direct  Kinematic  Equation  Development  for  the  Thumb 

For  the  thumb,  the  direct  kinematic  closed- form  equations  can  be 
developed  using 


Om  _  Om 
4H  ■  Vt 


(7.5) 


Now 


Om  _  Om 

■  vt 


(7.6) 
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where 


2m  _  2m  3m 

4H  “  an  ■  4‘t 


(7.7) 


Proa  (7.7),  we  have 


= 

4^ 


34 

-«34 

0 

^2  ^  ^3*^3 

‘34 

*^34 

0 

^3®3 

0 

0 

1 

0 

0 

0 

0 

1 

Further,  from  (7.6), 

Om  _  Om  Im 

AH  ~  iH  •  AH 


(7.8) 


where 


AH 


Im  2m 

2n  *  4^ 


(7.9) 


From  (7.9),  we  have: 


= 

aH 


'234 


‘234 


-  s 


234 

0 

'234 


0  ®1^^2‘^2^^3^23 

1  0 

0  ^2®2'^^3®23 

0  1 


(7.10) 


From  (7.8)  we  have 


= 

ih 


^1^234 

®  1^^234 
®234 

0 


<=18234 

®1®234 

^^234 

0 


0 

0 


®i(^1'^^2^2'^^3‘^23) 

^2®2^^3®23'^‘*1 


(7.11) 
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The  above  matrix  represents  the  position  of  the  origin  of  {4}  with 
respect  to  the  base  frame  {0}  and  the  orientation  of  the  last  link  with 
respect  to  the  base  frame  {0}. 

Position  of  Points  on  the  Last  Link 

The  position  of  any  point  on  the  last  link  of  the  thumb  which  is 
"L”  inches  from  the  origin  of  frame  {4},  is  given  by 

Pjj  =  Pj^  +  n^  .  L  where  p^  =  4T^(1,4)  and  n^ 

Py  =  Py  +  “y  •  I*  wherc  Py  =  4T^(2,4),  and  ny 

^  ”z  •  ^  Pz  =  \ 

Finger  Kinematics 

The  direct  kinematics  for  the  fingers  are  now  developed  with 
respect  to  the  common  base  frame. 

Denavit- Hartenburg  Parameters  for  the  Fingers 

Ve  now  proceed  to  develop  the  Denavit- Hartenburg  parameters  for  the 
fingers  of  the  Utah/MIT  dexterous  hand.  Ve  develop  these  parameters 
using  the  convention  outlined  in  Chapter  4.  Ve  have  also  adopted  the 
right-hand  rule  (counter-clockwise  positive)  for  determining  the  sign  of 
angles . 

Here,  aj  depends  on  the  finger  we  are  referring  to.  Thus, 
aj  =  -1.375"  for  finger  1,  aj  =  0"  for  finger  2  and  aj  =  1.1875"  for 
finger  3  for  the  left  hand. 

It  must  be  mentioned  here  that  the  frame  assignments  are  similar 
for  all  the  fingers,  and  so  the  kinematic  parameters  for  the  different 
fingers  vary  in  one  regard  only,  viz.  a^^  -  the  offset  along  the  palmar 

surface. 


=  X(3.1) 
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Table  6.  The  Denavit- Hartenburg  Parameters  for  the 
Fingers  of  the  Utah/MIT  Hand. 


i 

“i-1 

(degrees) 

1  ^i-1 

(inches) 

"i 

(inches) 

Joint 

Variable 

'i 

(degrees) 

Kinematic 
Kange  of  $• 
(degrees) 
i7,  14,  15 

1 

Oi=  -^®=  -12° 

-1.375” 
a.=  0" 
1.1875” 

Cos?  \ 
1.2COS30 

'i 

+65°  to^ 
+115° 

2 

O3  =  90° 

ai=  -0.6” 

da  =  0” 

>2 

+120° 
to  +191 

3 

02  =  0° 

»2  =  1-7'' 

da  =  0” 

+3.5° 
to  +90° 

4 

03  =  0° 

aa  =  1.3” 

d4  =  0” 

<-4 

0°  to  „ 

+90 

Finger  Transformation  Matrices: 

The  finger  transformation  matrices  are  obtained  using  the  general 
form  of  the  matrix  developed  in  Chapter  4  and  given  by  (4.6). 


Cl  -Si  0  a„ 
11  0| 

c^^i  s^  s^dj 

0  0  0  1 


Since  =  -  ^ 


a  =  a 

0  0, 


(f 


=  -12®)  (7.13) 

1,  2  or  3) 


^2  '  ®2  ^  ^1 
0  0-10 
S2  Cj  0  0 

0  0  0  1 


Since  =  90°  (7.14) 

and  d2  =  0 


T 


f 


C3  -  Sg  0  aj 

S3  C3  0  0 

0  0  10 

0  0  0  1 


Since  =  0° 
*  » 
and  dg  =  0 


(7.15) 
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-S4  0 

C4  0 

0  1 

0  0 


0 

0 

1 


Since  =  0°  (7.16) 

and  d4  =  0" 


Direct  Kinematic  Equation  Development  for  the  Fingers 

The  closed- form  direct  kinematic  equations  for  the  fingers  can  be 
developed  using 


(7.17) 


Using  the  process  of  transformation  matrix  concatenation,  we  have 

4ff  =  ?''f  •  2ff  •  ^f 


where 


2m  2m  3m 
Vf  -  3^f  •  4^f 


(7.19) 


From  (7.19),  we  solve  for  as 


4^f 


Om 

4^f 


where 


I? 

4^f 


^34 

'®34 

®34 

*^34 

0 

0 

0 

0 

we  can  write 

rf  • 

It 

4^f 

It 

2^f  • 

4^f 

0 

0 

1 

0 


^2^^3^3 

^3®3 

0 

1 


(7.20) 


(7.21) 


74 


Ve  therefore  compute  using  (7.21),  to  get 

*^234  ■  ®234  ® 

^  0  0-1 

®234  ^^234  ® 

0  0  0 

Again,  from  (7.20),  we  get 


*^1*^234 

■‘^1®234 

^®^®234 

■  Vi®234 
^®^^234 

'Vl 

^f- 

■^^^1*^234 

■^^^®234 

®^®1®234 

■"‘^^‘^234 

0 

0 

0 

1 

(7.23) 


^2®2^^3®23 


(7.22) 


The  above  matrix  represents  the  position  of  the  origin  of  {4}  and 
the  orientation  of  the  las^  link  of  the  fingers  with  respect  to  the  base 
frame  {0}. 


Positions  of  Points  on  the  Last  Link 

The  position  p’  of  any  point  on  the  last  link  of  the  fingers  which 
is  ’L’  inches  from  the  origin  of  frame  {4}  is  given  by 


p’  =  p  +  n  .  L 

*^X  *^X  X 

where  p^  =  ^7^(1, 4) 

and 

"x  = 

p’  =  p  +  n  .  L 

Fy  Fy  y 

where  p^  =  jTf(2,4) 

and 

”y  =  5Tf(2.1) 

(7.24) 

p’  =  P  +  n„  .  L 

where  p^  =  Jt^(3,4) 

and 

"x  ' 

Equations  (7.11),  (7.12),  (7.23) 

and 

(7.24)  represent 

the  direct 

kinematics  of  the  Ctah/MIT  dexterous  left  hand. 
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For  the  right  Utah/IIT  dexterous  hand,  the  direct  kinematics  can  be 
developed  in  one  of  two  possible  ways,  viz.  by  going  through  the 
procedure  outlined  above,  with  new  frame  assignments  for  the  right  hand, 
or  by  using  the  frames  assigned  previously  and  the  equations  developed 
above  and  adjusting  the  numeric  values  of  the  Denavit- Hartenburg 
parameters  to  correspond  to  a  right  hand. 

Ve  have  adopted  the  latter  procedure,  and  have  developed  and 
simulated  the  direct  kinematics  of  the  right  hand  by  changing  the  sign 
of  the  aQ  parameter  for  the  thumb  and  fingers.  Thus,  for  the  thumb, 

Uq  =  +  0.75“  (7.25) 

while  for  the  fingers,  the  value  of  Uq  is  given  by 
1.375”  for  finger  1 

a^v  =  0”  for  finger  2  (7.26) 
'^f  -1.1875"  for  finger  3 

The  above  adjusted  values  of  the  parameters  can  be  used  with  the 
direct  kinematic  equations  given  by  (7.23)  to  obtain  the  direct 
kinematics  of  the  right  Utah/IIT  dexterous  hand. 
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VIII  THE  SIMULATION  PROGRAM 


Introduction 

The  objective  of  this  computer  graphical  simulation  is  to  model  the 
kinematic  behavior  of  the  Merlin  6500  robot  arm  and  the  Utah/MIT 
dexterous  hand,  when  joined  together  in  user- defined  combinations.  The 
aim  is  to  allow  a  user  to  simulate  and  study  different  kinematic  joint 
arrangements  between  the  Merlin  robot  and  the  Utah  hand  and  move  the 
combined  systems  together  in  three  dimensional  space.  This  should 
provide  the  user  with  the  capability  of  investigating  various  strategies 
for  physically  attaching  the  Utah/MIT  hand  to  the  end  of  the  arm,  in 
terms  of  the  ranges  of  motion  and  also  the  total  workspace  of  the 
combined  system. 

Robot  Simulation 

The  simulation  adopts  the  methodology  of  depicting  links  by 
six-sided  figures,  thus  closely  approximating  the  actual  system.  Each 
of  the  links  consist  of  a  system,  and  are  connected  at  the  ends  to  other 
links  (or  systems)  by  joints.  All  of  the  joints  of  the  Utah/MIT 
dexterous  hand  (sixteen)  and  the  Merlin  manipulator  (six)  are  revolute 
in  nature. 

The  Link  Dimensioning  Approach 

The  method  used  to  graphically  depict  a  robot  involves  breaking  the 
subject  robot  into  sub-units  (links)  and  modeling  these  sub-units  using 
a  six-sided  (cubic)  figure  by  using  the  geometrical  spatial 
relationships  of  each  link’s  corners. 

It  is  assumed  that  each  link  can  be  represented  by  a  six  sided  box. 
The  box  is  dimensioned  to  be  able  to  closely  contain  the  dimensions  of 
the  real  link  being  represented.  It  thus  takes  eight  points  to  describe 
a  link.  Picture- perfect  accuracy  is  sacrificed  with  this  method,  but 
accurate  link  orientations  can  be  achieved.  The  points  are  measured 
vectorially  with  respect  to  a  local  origin  which  is  the  point  of 
rotation  or  translation  for  that  particular  link  of  the  manipulator. 
Each  link  of  a  robot  is  related  to  other  links  by  means  of  translation 
and  rotation  vectors  which  are  defined  with  respect  to  the  local  origin 
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of  the  link.  The  point,  translation  and  rotation  vectors  can  be  be 
stored  in  a  data  file  that  holds  all  the  information  necessary  to  model 
a  manipulator. 


Data  Files 

The  eight  point  vectors  defining  a  link  are  stored  in  a  specific 
manner.  The  first  row  of  eight  numbers  defines  the  I- coordinates  of  the 
eight  points  of  the  link.  The  second  row  defines  the  Y- coordinates, 
while  the  third  row  defines  the  Z- coordinates.  Thus  a  3  x  8  matrix  is 
needed  to  represent  each  link.  For  example,  a  link  may  be  defined  as  in 
Table  7. 


Table  7.  Link  Definition  in  Data  Files 


Corner  number 

Axes  i 

— .  1 

2 

3 

4 

5 

6 

7 

8 

X- coordinates 

8 

8 

8 

8 

0 

0 

0 

0 

Y- coordinates 

2 

2 

-2 

-2 

2 

2 

-2 

-2 

Z- coordinates 

2 

-2 

2 

-2 

2 

-2 

2 

-2 

Rotation  and  translation  vectors  are  also  stored  in  a  specific 
manner.  The  first  row  is  the  I,  Y  and  Z  coordinates  of  the  translation 
vector  while  the  second  row  is  the  rotation  vector,  consisting  of 
rotation  angles  about  the  I,  Y  and  Z  axes.  An  example  of  the  rotation 
and  translation  vectors,  as  stored  in  the  data  file,  is  given  in 
Table  8. 

Once  the  dimensions  for  each  link  of  a  system  is  stored  in  a  data 
file  in  a  form  that  can  be  read  into  the  program,  the  system  is  defined 
for  the  purposes  of  modeling.  Point  vectors  are  stored  in  order  of  link 
sequence  as  one  group,  while  translation  and  rotation  vectors  are  stored 
in  order  of  link  sequence  as  another  group.  In  the  main  program,  arrays 
are  dimensioned  to  store  the  dimensional  values  of  a  link.  A  data  point 
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Table  8.  Rotation  and  Translation  Vectors  Stored  in  Data  Files 


Coordinate 

I 

Y 

z 

II 

II 

It 

Translation  vector 

30 

0 

5 

Rotation  angles 

90° 

0° 

0° 

array  (an  example  is  POINTS)  is  dimensioned  as  P0INTS(8,3,I,J) ,  where  8 
denotes  the  number  of  points  in  each  link,  3  the  number  of  coordinate 
dimensions,  I  the  number  of  links  in  the  system  and  J  the  number  of 
systems.  The  translational/  rotational  array  (an  example  is  60)  could 
be  dimensioned  as  60(3, I, J,K),  where  3  denotes  the  number  of  dimensions, 
1=1  the  translational  vector  and  1=2  the  rotational  vector,  J  the 
number  of  links  and  K  the  number  of  systems. 

Link  Dimensionine; 

The  robot  model  is  made  up  of  a  series  of  links,  for  example  the 
base  link,  stand,  waist,  upper  arm  and  lower  arm.  The  origin  of  each 
link  is  the  point  of  rotation  of  the  joint.  The  dimensional 
relationship  between  each  link  must  be  defined.  The  notation  necessary 
to  relate  one  link  to  the  previous  link  is  a  translation  vector  and  a 
rotation  vector.  The  link  translation  vector  originates  at  the  previous 
link  in  the  open  kinematic  chain  and  ends  at  the  origin  of  the  current 
link.  It  is  defined  with  respect  to  the  previous  link’s  coordinate 
system.  The  rotation  vector  relates  the  orientation  of  the  current 
coordinate  system  to  the  previous  coordinate  system. 

Rotational  transformation  must  occur  in  a  fixed  order,  viz. 
rotation  about  the  X-axis  first,  followed  by  rotation  about  the  Y-axis 
and  finally,  if  necessary,  about  the  Z-axis.  (The  Z- rotation  component 
is  currently  left  as  zero,  thereby  saving  that  component  for  use  as  a 
dynamic  rotation  in  the  program).  For  a  link  with  a  coordinate  system 
translated  30  units  in  the  I- direction  and  offset  5  units  in  the 
Z- direction,  with  a  90  degree  rotation  about  the  X-axis,  the  vectors 
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would  be  represented  in  a  data  file  as  in  table  9. 


Table  9.  Link  Translations  and  Rotation  Angles. 


Coordinate 

X 

Y 

z 

tt 

ft 

ff 

Translation 

30 

0 

5 

Rotation  angles 

90® 

0° 

0° 

The  first  row  defines  the  translational  vector  components  in  the  X, 
Y  and  Z  coordinate  directions  and  the  second  row  defines  the  rotation 
angles  about  the  X,  Y  and  Z  axes.  These  values  are  used  to  fill  the 
transformation  matrix  for  a  specific  link. 

Coordinate  System  Transformation 

The  orientation  of  each  link  is  defined  with  respect  to  the 
previous  link  using  rotation  and  translation  vectors.  The  values  of  the 
vectors  are  measured  with  respect  to  the  previous  coordinate  system. 
The  vectors  are  loaded  into  a  transformation  matrix  by  using  input  data 
from  the  matrix  [GO]  to  form  the  transformation  matrix  [TR] .  The 
following  equations  are  used  to  determine  each  component  of  the  matrix 
[TR] 


G0(1,1,L,S) 

G0(2,1,L,S) 

G0(3,1,L,S) 

G0(1,2,L,S) 

G0(2,2,L,S) 

G0(3,2,L,S) 


Translation  in  the  X  direction 
(L  is  the  link  number  and  S  is  the  system  number) 
Translation  in  the  Y  direction 
Translation  in  the  Z  direction 
Rotation  about  the  X  axis 
(Counterclockwise  is  deemed  positive), 

Rotation  about  the  Y  axis 
Rotation  about  the  Z  axis 


The  transformation  matrix,  TR,  is  formed  using  : 

TR(1,1,L,S)  =  C0S(G0(3,2,L,S;)  *  C0S(G0(2,2,L,S)) 
TR(1,2,L,S)  =  -SIN(G0(3,2,L,S))  ♦  C0S(G0(2,2,L,S)) 


TR(l,3,l,S)  =  SIN(G0(2,2,L,S)) 

TR(l,4,l,S)  =  60(1,1,L,S) 

TR(2,l,l,S)  =  C0S(G0(3,2,L,S))  ♦  SIN(G0(2,2,L,S))  ♦ 

SIN(G0(1,2,L,S))  +  SIN(G0(3,2,L,S))  ♦  C0S(G0(1,2,L,S)) 
TR(2,2,l,S)  =  -SIN(G0(3,2,L,S))  ♦  SIK(G0(2,2,L,S))  * 

SIN(G0(1,2,L,S))  +  C0S(G0(3,2,L,S))  *  C0S(G0(1,2,L,S)) 
TR(2,3,l,S)  =  -C0S(G0(2,2,L,S))  *  SIN(G0(l,2,l,S)) 

TR(2,4,L,S)  =  G0(2,1,L,S) 

TR(3,1,L,S)  =  -CflS(G0(3,2,L,S))  ♦  SIN(G0(2,2,L,S))  * 

C0S(G0(1,2,L,S))  +  SIN(G0(3,2,L,S))  ♦  SIN(G0(1,2,L,S)) 
TR(3,2,l,S)  =  SIN(G0(3,2,L,S))  ♦  SIN(G0(2,2,L,S))  * 

C0S(G0(l,2,l,S))  +  C0S(G0(3,2,L,S))  *  SIN(G0(1,2,L,S)) 
TR(3,3,L,S)  =  C0S(G0(l,2,l,S))  *  C0S(G0(2,2,L,S)) 

TR(3,4,L,S)  =  G0(3,l,l,S) 

TR(4,1,L,S)  =  0 
TR(4,2,L,S)  =  0 
TR(4,3,L,S)  =  0 
TR(4,4,L,S)  =  1 

The  notation  used  for  transformation  from  {B}  to  {A}  is  BTA  (B 
transformed  to  A).  To  describe  points  in  {C}  with  respect  to  {A},  the 
transformation  matrix  BTA  must  be  premultiplied  by  the  transformation 
matrix  CTB.  This  operation  is  performed  in  each  link  so  that  points  in 
each  link  can  be  defined  in  the  base  or  global  coordinate  system.  Ve 
need  to  know  the  definition  of  the  coordinates  of  a  point  defined  in  {C} 
in  the  coordinate  system  of  {A}.  To  find  this,  it  is  necessary  to 
multiply  the  transformation  matrix  CTA  by  the  vector  defining  the  point 
in  {C}. 

To  get  the  vector  coordinates  of  a  point  in  a  link  in  the  global 
system,  the  (4X4)  transformation  matrix  is  multiplied  by  a  (4X1)  vector 
composed  of  the  three  coordinates  of  the  point  and  a  1  in  the  fourth 
row.  The  result  of  the  product  is  the  definition  of  the  vector  in  the 
globa]  system.  This  operation  has  to  be  performed  for  all  links  in  alJ 
systems. 

Once  the  dimensions  of  each  point  of  each  link  are  known  in  the 
global  system,  the  three  dimensional  points  must  be  transformed  into  two 
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dimensional  screen  drawing  points. 

A  perspective  view  is  desired,  where  objects  appear  to  be  shrinking 
with  increasing  distance.  This  requires  choosing  a  focal  point,  defined 
in  global  I  and  Y  coordinates,  and  a  viewing  point  (or  VPOINT  -  a 
distance  along  the  global  Z  axis). 

A  drawn  view  really  has  tunnel  vision,  it  can  only  ’see’  objects 
that  are  within  a  20  degree  cone  directly  in  front  of  the  viewer. 
Imagine  two  lines,  one  drawn  from  the  viewpoint  to  the  global  origin  and 
the  other  drawn  from  the  viewpoint  to  a  point  defining  a  link.  If  this 
angle  between  these  two  lines  is  zero,  the  point  will  be  assigned  2-D 
coordinates  of  (0,0).  If  the  angle  is  equal  to  20  degrees,  the  2-D 
coordinates  will  be  assigned  coordinates  corresponding  to  the  edge  of 
the  screen.  If  the  angle  lies  between  0  and  20  degrees,  the  assigned 
2-D  coordinates  will  be  assigned  proportional  to  the  angle.  If  the 
angle  is  greater  than  20  degrees,  then  the  point  will  not  be  shown  in 
the  view. 

Once  the  2-D  coordinates  of  all  points  are  known,  a  graphics 
package  can  be  employed  to  connect  lines  between  the  appropriate  points 
so  that  the  cubes  defining  the  links  can  be  drawn.  The  robot  thus 
consists  of  a  series  of  links  assembled  together  at  the  joints. 

The  Graphics  Software  Menus 

The  graphics  package  operates  using  a  VT  240  or  a  Tektronix  4010 
screen.  It  is  organized  to  be  user-friendly,  and  thus  incorporates  a 
main  menu  and  sub- menus.  The  organization  of  the  menus  and  sub- menus 
follows  a  logical  pattern  determined  by  user  operations.  The  program 
begins  with  a  main  menu  and  two  sub- menus.  The  main  menu  is  the  entry 
point  for  both  the  sub- menus  as  well  as  the  program  exit  point.  One  of 
the  options  in  the  main  menu  also  allows  the  user  to  see  the  system,  as 
currently  defined,  on  the  screen. 

Major  sub- menus  consist  of  the  setup  and  operations  menus.  The 
set-up  sub- menu  allows  the  user  to  perform  the  tasks  of  defining  the 
system  in  terms  of  the  viewpoint,  the  focal  point  and  the  factor  of 
magnification  for  subsequent  views,  which  of  the  possible  systems  (the 
room,  left/right  arm  Merlin,  left/right  Dtah/MIT  dexterous  hand)  are  to 
be  drawn,  the  relative  position  of  the  Dtah/MIT  hands  with  respect  to 
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the  end  of  the  lerlin  arms,  the  positioning  of  the  robots  in  the  room, 
drawing  the  systems  as  curre:.i.ly  defined,  which  of  the  two  possible 
remote  slave  systems  is  to  be  currently  active  (only  one  slave  system  is 
active  at  any  one  time)  and  finally,  returning  to  the  main  menu. 

The  Execution  sub- menu  allows  a  user  to  move  the  individual  joints 
of  the  selected  systems,  viz.  the  lerlin  and  the  Utah/MIT  hand,  to  save 
a  view  and  to  recall  a  saved  view,  to  move  the  lerlin  from  its  current 
position  to  another  point,  defined  by  its  position  and  orientation,  to 
move  all  joints  of  the  robot,  in  user- defined  steps,  and  to  return  to 
the  main  menu. 

Each  item  in  each  of  the  menus  is  discussed  in  more  detail  below. 
The  Main  Menu 

The  user  chooses  one  of  the  following  options  from  the  main  menu 

Go  to  the  setup  menu. 

Go  to  the  execution  menu. 

Draw  the  system,  as  currently  defined. 

Exit  from  the  program. 

The  Setup  Menu 

The  set-up  menu  consists  of  the  following  options,  each  of  which  is 
explained  below  ; 

VIEVPOIHT 

The  position  in  global  coordinates  the  the  robot  is  to  be  viewed 
from  is  chosen  by  this  operation.  The  global  origin  is  located  at  the 
lower,  far  left  corner  of  the  room.  On  the  screen,  ’X’  is  positive 
towards  the  right,  ’Y’  is  positive  towards  the  top  and  ’Z’  is  positive 
coming  out  of  the  screen. 

FOCUS  IND  lAGMIFICATIOlf 

The  focal  point  is  defined  in  screen  coordinates  as  (0,0)  and  is  in 
the  center  of  the  screen.  The  focal  point  is  not  in  global  coordinates, 
which  factor  may  cause  confusion  when  the  viewpoint  is  changed  and  the 
focal  point  is  not  (0,0).  Each  view,  no  matter  what  the  viewpoint,  has 
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a  unique  screen  focal  point,  i.e.  (0,0).  Any  new  focal  point  for  a 
specific  view  is  measured  with  respect  to  the  focal  point  at  (0,0). 

The  user  may  define  the  magnification  for  a  specific  view. 
Increasing  the  magnification  will  make  the  objects  being  viewed  appear 
larger.  A  point  in  the  center  of  the  screen  will  remain  in  the  center 
of  the  screen. 

SYSTEIS  D1AVIN6 

The  following  are  defined  as  systems: 

The  Room  in  which  the  slave  systems  are  placed, 

A  Left- Arm  Merlin  Robot, 

A  Left  Utah/MIT  dexterous  hand, 

A  Right- Arm  Merlin  Robot, 

A  Right  Utah/MIT  dexterous  hand. 

Prior  to  any  specific  view,  the  user  may  choose  to  enable  or 
disable  the  drawing  of  any  of  the  above  systems.  After  selecting  this 
option,  a  menu  is  displayed,  prompting  the  user  to  choose  one  of  the 
available  options  related  to  drawing  (or  not  drawing)  a  system. 

FIXED  MAND  POSITIONING 

The  fixed  position  of  the  Utah/MIT  hand  relative  to  the  Merlin 
wrist  may  be  changed  by  the  user.  The  base  coordinate  system  of  the 
hand  is  related  to  the  wrist  coordinate  system  by  a  translation  vector 
and  a  90  degree  rotation  about  the  ’X’  axis  of  the  wrist  coordinate 
system.  The  hand  can  be  positioned  with  respect  to  the  robots  hand  roll 
system  by  a  translation  vector  and  a  rotation  vector.  After  selecting 
this  option,  the  user  is  prompted  to  define  these  vectors. 

REPOSITION  ROBOT 

The  robot  may  be  positioned  anywhere  in  the  room.  The  origin  of  the 
room  coordinate  system  is  the  far  left,  bottom  corner  of  the  room  as 
seen  in  the  initial  view.  The  position  of  the  robot  is  defined  by  a 
vector  from  the  origin  of  the  room  to  the  center  of  the  base  of  the 
robot.  The  program  prompts  the  user  for  input  necessary  in  redefining 
this  vector. 
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CliHGE  ACTIYE  lOBOT 

Robot  positioning  and  reconfiguring  is  perfoned  working  from  the 
■enus  during  the  operation  of  the  program.  It  is  possible  for  two 
robots  to  be  viewed  at  the  same  time.  However,  only  one  robot  is  deemed 
active  at  any  one  time.  This  option  allows  the  user  to  choose  which 
robot  is  the  one  to  be  acted  upon  at  any  one  time.  This  allows  for 
multiple  arms  to  be  used  in  simulations. 

lETUlH  TO  lAIH  KNU 

Choosing  this  option  returns  the  user  to  the  main  menu. 

DUV  tOBOT 

This  option  provides  a  view  of  the  systems,  as  currently 
configured. 

The  Execution  Menu 

The  execution  menu  possesses  several  options,  each  of  which  are 
discussed  below  : 

lOVE  lOBQT  JOINT  INGLES 

Each  of  the  joint  angles  of  the  Merlin  robot  can  be  moved 
individually.  Once  this  option  is  chosen,  another  menu  will  be 
displayed  pr(  •  iting  the  user  to  choose  the  link  to  be  repositioned. 
Once  a  link  is  chosen,  the  user  is  informed  of  the  current  angle  and  is 
prompted  for  the  desired  joint  angle  in  degrees.  After  input,  the  link 
menu  is  displayed  again  until  the  user  requests  an  exit  from  that  menu. 

MOVE  INDIVIDUAL  FINGER  JOINTS 

Each  of  the  Utah/MIT  hand’s  joint  angles  can  be  moved  individually. 
Once  this  option  is  chosen,  another  menu  is  displayed  prompting  the  user 
to  choose  the  finger  to  be  repositioned.  Once  a  finger  is  chosen,  a 
third  menu  is  displayed  prompting  the  user  to  choose  the  joint  to  be 
repositioned.  Next  the  user  is  informed  of  the  current  specified  joint 
angle  and  is  prompted  for  the  desired  joint  angle  in  degrees.  After 
input,  the  joint  menu  is  displayed  again  until  the  user  requests  to  exit 
that  menu,  following  which  the  finger- choosing  menu  is  displayed  until 


an  exit  is  requested. 


SAVE  TIIS  YIEV 

If  a  user  wishes  to  leave  his  work  and  resume  later  at  the  ending 
point  of  the  last  session,  this  option  will  store  the  current  parameters 
that  define  the  configuration  of  the  present  view.  These  parameters  are 
written  to  a  file  named  ’SAVE.DAT’.  Only  one  view  can  be  stored  during 
the  course  of  a  program  run.  A  second  save  will  write  over  the  first 
save.  After  leaving  the  program,  the  save  file  could  be  renamed  to 
avoid  being  written  over  by  a  future  run.  If  this  renamed  file  is  to  be 
used  in  a  program,  it  will  need  to  be  copied  to  ’SAVE.DAT’  prior  to 
running  the  program. 

DiAV  KOBOT  FKOI  SAVED  DATA  FILE 

The  user  may  resume  work  from  a  previously  saved  parameter  file. 
After  selecting  this  option,  the  next  view  drawn  will  be  defined  by 
parameters  read  frum  a  file  named  ’SAVE.DAT’. 

MOVE  lOBOT  TIP  POINT  TO  POINT 

This  option  uses  the  inverse  kinematics  of  the  Merlin  robot  arm  to 
move  the  tip  of  the  Merlin  arm  from  the  current  position  to  the  user 
defined  position.  The  goal  (or  desired)  position  is  chosen  by 
specifying  the  desired  position  and  orientation  of  the  tip  of  the  Merlin 
manipulator,  relative  to  the  global  frame  of  the  Merlin  (defined  in 
Chapter  5).  The  user  also  has  to  select  the  desired  joint  angles  from 
the  computed  set  of  valid  angles  that  are  determined  by  the  inverse 
kinematic  computation.  The  file  INKIN. FOR  has  to  be  compiled  and  linked 
to  the  simulation  program  for  this  option  to  be  active. 

MOVE  AIL  JOINTS  OF  THE  ROBOT,  IN  STEPS 

The  user  will  be  prompted  to  enter  a  complete  set  of  six  joint 
angles  and  the  number  of  steps.  The  user  also  chooses  to  either  erase 
between  views,  or  to  draw  each  view  on  the  same  screen. 

Figure  14  shows  a  succession  of  positions,  detailing  the  robot’s 
movements  from  the  current  position  to  the  position  defined  by  the  set 


86 


of  angles  that  were  entered  by  the  user.  The  number  of  views  is 
determined  by  the  number  of  steps.  Intermediate  angles  are  computed  by 
interpolation  of  the  initial  and  final  angles. 
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Figure  14.  The  Computer  Graphical  Program 
-  Simulation  Results. 
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II  CONCLUSIONS 


The  Results 

This  study  has  presented  the  methodology  and  mathematics  of 
kinematic  analysis  in  chapters  I,  II,  III  and  IV,  followed  by  the 
derivation  of  the  direct  and  inverse  kinematic  closed- form  equations  for 
the  Merlin  6500  left-  and  right-  shouldered  manipulator  in  chapter  Y.  A 
graphical  simulation  proceedure  and  results  of  the  planar  workspace  for 
the  Merlin  6500  left- shouldered  robot  arm  is  detailed  in  chapter  VI. 
The  study  then  proceeds  to  examine  the  development  of  the  direct 
kinematic  closed- form  equations  for  the  Utah/MIT  dexterous  hand  in 
chapter  VII.  This  is  followed  by  a  discussion  of  the  computer  graphical 
simulation  for  the  Merlin  and  Dtah/MIT  dexterous  hand,  when  combined  in 
user  defined  configurations,  detailed  in  chapter  VIII. 

Further  Work 

The  current  study  has  examined  only  the  direct  kinematics  of  the 
Utah/MIT  dexterous  hand.  As  the  slave  system  is  driven  from  a  remote 
location  by  a  human  arm  encased  in  an  exo- skeleton,  and  since  it  is 
necessary  for  the  slave  system  to  grasp  objects  at  the  same  position  and 
with  the  same  orientation  as  the  driving  master  system,  it  becomes 
necessary  to  perform  kinematic  transformations  between  the  master  and 
the  slave  systems.  These  transformations  will  typically  involve  the 
direct  kinematics  of  the  human  hand,  whose  output  data  can  be  utilized 
as  an  input  to  the  inverse  kinematics  of  the  Utah/MIT  hand,  thus 
allowing  for  objects  to  be  grasped  by  the  slave  system  in  a  similar 
fashion  to  the  master  system.  Thus,  a  study  of  the  kinematic  mapping 
between  the  human  hand  and  the  Utah/MIT  hand  must  be  made,  so  that  when 
the  activating  system  (the  human  arm)  grasps  an  object,  the  remote 
system  follows  it  in  action.  lastly,  there  is  a  need  to  study  the 
mechanism  of  object  grasping  by  the  human  hand  as  well  as  by  the 
Otah/MIT  hand,  for  different  object  geometries.  This  will  permit  the 
Utah/MIT  hand  to  grasp  and  manipulate  the  remote  object  in  as  dexterous 
a  fashion  as  the  human  arm. 

The  current  study  notes  the  existence  of  the  remotizer  of  the 
Utah/MIT  dexterous  hand  and  the  constraint  it  poses  for  the  operation  of 
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the  renote  system.  A  further  study  must  also  deal  with  the  effect  of 
the  remotizer  on  the  use  of  the  Utah/lIT  hand  as  an  end- effector  for  the 
slave  system.  This  would  typically  involve  studying  the  effect  of  the 
remotizer  on  the  workspace  of  the  remote  slave  system,  such  that  the 
workspace  would  be  a  maximum  without  undue  effect  on  the  remotizer 
links. 
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APPENDIX  1 


DEFINITIONS 

In  any  scientific  study,  it  is  necessary  to  make  clear  the  meaning 
of  certain  technical  words  that  are  being  used,  so  as  to  avoid  confusion 
in  their  use  by  different  users  with  varying  backgrounds.  As  such, 
certain  key  words  used  in  this  study  are  explained  below 

Al.l  Kinematics  Kinematics  is  the  science  of  motion  which  treats 
motion  without  regard  to  the  forces  that  cause  it.  Vithin  the  science 
of  kinematics,  one  may  study  the  geometrical  properties  of  motion  or  the 
time  derivatives  of  position.  Ve  limit  the  present  study  to  the 
geometrical  properties  of  motion. 

A1.2  Manipulator  (or  Robot)  A  manipulator  is  kinematically  defined  to 
be  a  set  of  nearly  rigid  links  connected  together  in  a  chain  by  joints 
which  allow  relative  motion  of  the  neighbouring  links.  In  the  case  of 
rotary  or  revolute  joints,  the  displacements  are  joint  angles,  while  in 
the  case  of  sliding  or  prismatic  joints,  these  displacements  are  joint 
offsets. 

Al.3  Degrees  of  freedom  The  degrees  of  freedom  present  at  any 
joint  of  a  mechanism  are  computable  as  the  number  of  independent 
position  variables  that  need  to  be  specified  to  locate  specific  parts  of 
the  mechanism.  In  the  case  of  typical  industrial  manipulators,  since 
such  a  manipulator  is  usually  an  open  kinematic  chain,  and 
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because  each  joint  position  is  usually  defined  with  a  single  variable, 
the  number  of  joints  equals  the  number  of  degrees  of  freedom.  Each 
joint  may,  however,  possess  one  or  more  degrees  of  freedom. 

Al.4  Frame  A  frame  is  defined  to  be  a  co-ordinate  system  attached  to 
a  joint  of  a  manipulator.  The  end- frame  is  generally  attached  to  the 
tip  of  the  manipulator  or  to  the  last  joint  of  the  open  kinematic  chain, 
while  the  base  frame  is  generally  attached  to  a  non- moving  component  of 
the  manipulator.  In  this  report,  a  frame  is  always  referenced  by  the 
character  inside  {}. 

A1.5  Cartesian  space  is  defined  as  the  space  in  which  the  position 
of  a  point  is  given  by  three  position  data  values  along  the  three 
orthogonal  axes,  X,  Y  and  Z,  while  the  orientation  of  a  body  is  given  by 
three  orientation  data  value  sets. 

Al.6  Joint  Space  is  defined  as  the  space  in  which  the  position  of  a 
point,  and  the  orientation  of  a  link,  are  defined  in  terms  of  the  joint 
variable  (or  degrees  of  freedom). 

Al.7  Forward  Kinematics  The  forward  (or  direct)  kinematic  problem  is 
defined  to  be  the  computation  of  the  position  and  orientation  of  the 
end- effector  frame  relative  to  the  base  frame.  This  problem  can  also  be 
thought  of  as  changing  the  representation  of  manipulator  position  and 
orientation  from  a  joint  space  description  into  a  Cartesian  space 
description. 
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Al.8  Inverse  Kinematics  The  inverse  kinematics  problem  is  defined  to 
be  the  computation  of  the  joint  variables  when  the  position  and 
orientation  of  the  end- effector  frame  is  known  with  respect  to  the  base 
frame.  This  problem  can  also  be  thought  of  as  changing  the 
representation  of  manipulator  position  and  orientation  from  a  Cartesian 
space  description  to  a  joint  space  description. 

A1.9  Workspace  The  work- space  of  a  manipulator  is  defined  as  the 
set  of  positions  which  the  end- effector  can  achieve  when  the  joints 
degrees  of  freedom  vary  over  the  full  range  of  possible  values. 

Al.lO  Reachable  work- space  The  reachable  work- space  of  a  manipulator 
is  defined  as  that  volume  of  space  which  the  robot  end- effector  can 
reach  in  at  least  one  orientation. 

Al.ll  Dextrous  work- space  The  dexterous  work- space  of  a  manipulator 
is  defined  as  the  volume  of  space  which  the  robot  end- effector  can  reach 
with  all  possible  orientations.  The  dexterous  work- space  of  a 
manipulator  is  always  a  sub- set  of  the  reachable  work- space  of  that 
manipulator. 
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APPENDIX  2 


ROTATION  AND  TRANSLATION  MATRICES 

In  chapter  IV,  we  derived  the  general  form  of  the  transformation 
^^^T  by  using  equation  (4.1)  to  (4.5).  Equation  (4.4)  involved 
rotations  and  translations  about  the  X^  and  axes.  These  rotation  and 
translation  matrices  are  shown  below,  in  general  terms. 


Let  us  first  examine  a  rotation  of  a  about  the  X  axis.  The 
transformation  matrix  is  given  by 


Rot(X,  a)  =  0 


Cos  a 


Sin  a 


Sin  a 


Cos  a 


A  translation  along  the  X  axis  by  a  distance  of  ’a’  is  given  by 


Trans (X,  a) 


A  rotation  about  the  Z  axis  by  an  angle  9°  is  given  by 


Cos  0  -Sin  9 


Rot(Z,  0)  =  Sin  9  Cos  9 
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A  translation  along  the  Z  axis  for  a  distance  ’d’  is  given  by 


0 


Trans (Z,  d) 


0 

0 


0  0 

0  0 

0  d 


Therefore,  in  equation  (4.5),  if  we  substitute  the  appropriate  form 
of  the  rotations  and  translations,  viz.  a  rotation  about  the  axis  by 
1  degrees  and  a  translation  along  the  axis  by  a^  ^  (i.e.  a 
Screw{X^,  j^}),  and  a  rotation  about  Z^  by  0^  degrees  and  a 

translation  along  the  Z^  axis  by  a  distance  d^  (i.e.  a  Screw{Zp 
d^}),  and  multiply  out  equation  (4.5),  we  get  equation  (4.6),  as  follows 
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APPENDIX  3 


DIRECT  KINEMATICS  SIMULATION  FOR  THE  MERLIN  6500  -  LEFT  ARM 


cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

C  MERLIN  ROBOT  LEFT  ARM  KINEMATICS  SIMULATION  PROGRAM 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

C  MAIN  PROGRAM 

C  DEFINE  REAL  ft  INTEGER  VARIABLES 
INTEGER  INTRO 

REAL  T(4,4),V,S,E,VR,VP,HR 
C  DESCRIBE  THE  PROGRAM 


)(5,*)  INTRO 
[NTRO  .EQ.  1)  THEN 


PRINT  THIS  PROGRAM  PERFORMS  A  MATHEMATICAL  SIMULATION  OF’ 
PRINT  THE  KINEMATICS  OF  THE  MERLIN  6500  LEFT  ARM  ROBOT.’ 

10  PRINT  ’ 

PRINT  DO  YOU  NEED  AN  INTRODUCTION  TO  THE  PROGRAM  ?  ’ 

PRINT  YES  - >  1.’ 

PRINT  NO  ---->2.’ 

INTRO  =  2 
READ(5,*)  INTRO 
IF (INTRO  .EQ.  1)  THEN 
CALL  INTROD 

ELSE  IF(INTRO  .EQ.  2)  THEN 
GOTO  11 

ELSE 

PRINT  ENTRY  ERROR’ 

GOTO  10 
ENDIF 

C  FIND  THE  USER- DEFINED  ANGLES 

11  CALL  ANGLES(V,S,E,VR,VP,HR) 

PRINT  ’ 

CALL  DIRKIN(V,S,E,VR,VP,HR,T) 

CALL  TOUT(T) 

STOP 

END 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
C  INTRODUCTION  TO  THE  PROGRAMME 
SUBROUTINE  INTROD 

PRINT  THE  program  REQUESTS  THE  USER  TO  ENTER  THE  JOINT’ 
PRINT  *  ’  ANGLES  FOR  EACH  OP  THE  FOLLOWING  JOINTS 


PRINT 

PRINT 

PRINT 

PRINT 

PRINT 

PRINT 

PRINT 

PRINT 

PRINT 

PRINT 

PRINT 

PRINT 

PRINT 


THE  program  REQUESTS  THE  USER  TO  ENTER  THE  JOINT’ 
ANGLES  FOR  EACH  OP  THE  FOLLOWING  JOINTS 


JOINT 

WAIST  JOINT 
SHOULDER  JOINT 
ELBOW  JOINT 
WRIST  ROLL 
WRIST  PITCH 
TOOL  ROLL 


RANGE  ’ 
__  > 

RANGE  +  147  TO  - 
RANGE  +  56  TO  - 
RANGE  +  56  TO  - 
RANGE  +  360  TO  - 
RANGE  +  90  TO  - 
RANGE  +  360  TO  - 


147  DEGREES 
236  DEGREES 
236  DEGREES 
360  DEGREES 
90  DEGREES 
360  DEGREES 


THE  PROGRAM  RETURNS  THE  FINAL  TRANSFORMATION’ 
MATRIX  i.e.  THE  POSITION  ft  ORIENTATION  MATRIX’ 
DEFINED  AT  THE  WRIST  PIN  OR  TIP  OF  THE  ARM. ’ 


ELL URN 
^ND 

CC'':CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
C  ENTRY  OF  JOINT  ANGIES  BY  THE  USER 
SUBROUTINE  ANGLES (V , S , E , VR , VP , HR) 

REAL  V1,S1,E1,VR1,VP1,HR1,V,S,E,VR,VP,HR,PI 
C  DEFINE  PI 

PI  =  3.141592654 
C  VAIST 

101  V  =  0.0 
VI  =  0.0 

PRINT  * ENTER  VAIST  ANGLE  (  +/-  147  DEGREES  )  ==>  ’ 

READ (5,*)  VI 

IF  (ABS(Vl)  .GT.  147.0)  THEN 

PRINT  ERROR  --  RANGE  IS  +/-  147  DEGREES.  ’ 

GOTO  101 
ENDIF 

V  -  VI  *  PI  /  180.0 
C  SHOULDER 

102  S  =  0.0 
SI  =  0.0 

PRINT  *  ’  ENTER  SHOULDER  ANGLE  (  56  TO  -236  DEGREES  )  -=>  ’ 
READ(5,*)  SI 

IF  ((Si  .GT.  56.0)  .OR.  (Si  .LT.  -236.0))  THEN 
PRINT  ERROR  --  RANGE  IS  56  TO  -236  DEGREES.  ’ 

GOTO  102 
ENDIF 

S  =  SI  *  PI  /  180.0 
C  ELBOV 

103  E  =  0.0 
El  =  0.0 

PRINT  * ENTER  ELBOV  ANGLE  (  56  to  -236  DEGREES  )  ==>  ’ 
READ(5,*)  El 

IF  ((El  .GT.  56.0)  .OR.  (El  .LT.  -236.0))THEN 
PRINT  ERROR  --  RANGE  IS  56  TO  -236  DEGREES.  ’ 

GOTO  103 
ENDIF 

E  =  El  *  PI  /  180.0 
C  VRIST  ROLL 

104  VR  =  0.0 
VRl  =  0.0 

PRINT  *  ’  ENTER  VRIST  ROLL  (  +/-  360  DEGREES  )  ==>  ’ 

READ(5/)  VRl  ^ 

IF  (ABS(VRl)  .GT.  360.0)  THEN 
PRINT  ERROR  --  RANGE  IS  +/-  360  DEGREES.  ’ 

GOTO  104 
ENDIF 

VR  =  VRl  *  PI  /  180.0 
C  VRIST  PITCH 

105  VP  =  0.0 
VPl  =  0.0 

PRINT  ENTER  VRIST  PITCH  (  +/-  90  DEGREES)  -=>  ’ 

READ(5,*)  VPl 

IF  (ABS(VPl)  .GT.  90.0)  THEN 
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PRINT  ERROR  -  RANGE  IS  +/-  90  DEGREES.  ’ 

GOTO  105 
ENDIF 

VP  =  VPl  *  PI  /  180.0 
C  HAND  ROLL 
106  HR  =  0.0 
HRl  =  0.0 

PRINT  * ENTER  HAND  ROLL  (  +/-  360  DEGREES  )  ==>  ’ 

READ(5,=^)  HRl 

IF(ABS(HR1)  .GT.  360.0)  THEN 
PRINT  ERROR  --  RANGE  IS  +/-  360  DEGREES.  ’ 

GOTO  106 
ENDIF 

HR  =  HRl  *  PI  /  18C.0 

RETURn 

END 

ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

C  KINEMATICS  IMPLEMENTATION 

SUBROUTINE  BIRKIN(V,S,E,VR,VP,HR,T) 

INTEGER  I,J,TIP 

REAL  T(4,4),C1,C2,C3,C4,C5,C6,S1,S2,S3,S4,S5,S6,C23,S23,D2,D3, 
$R11A,R11B,R12A,R12B,R13A,R13B,R14A,R14B,V,S,E,VR,VP,HR,D6 
C  INITIALIZE  MATRIX 
DO  301  I  =  1,4 
DO  301  J  =  1,4 
T(I,J)  =  0.0 
301  CONTINUE 

T(4,4)  =  1.0 

C  DEFINE  COSINES  AND  SINES 
Cl  =  COS(V) 

C2  =  COS  S 
C3  -  COS(E) 

C4  =  COS  VR) 

C5  COS(VP) 

C6  -  COS  HR) 

51  =  SIN  V] 

52  -  SIN  S) 

53  =  SIN(E) 

54  =  SIN(VR) 

55  ^  SIN (VP) 

56  --  SIN  (HR) 

C23  -  COS(S  ^  E) 

S23  =  SIN(S  ^  E) 

C  DEFINE  D2,  D3  i  D6.'  APPROXIMATELY  )  FOR  THE  LEFT  ARM. 

C  D2  IS  THE  SIGNED  DISTANCE  FROM  THE  GLOBAL  X  AXIS  TO  THE  SHOULDER 
C  CENTRAL  AXIS  (  ALIGNED  ALONG  THE  UPPER  ARM  ) .  D3  IS  THE  SIGNED 
C  DISTANCE  FROM  THE  SHOULDER  CENTRAL  AXIS  TO  THE  ELBOV  CENTRAL 
C  AXIS,  ALIGNED  VITH  THE  CENTER  OF  THE  LOVER  ARM.  THESE  VALUES 
C  {D2  t  D3)  CHANGE  SIGN  FOR  THE  RIGCT  ARM.  D6  IS  THE  SIGNED 
C  DISTANCE  FROM  THE  VRTST  PIN  TO  THE  TIP  OF  THE  ROBOT  ARM. 

C  (  REFER  TO  ARM  KINEMATICS  FOR  MORE  DETAILS.  ) 

D2  =  19.00 
D3  -  7.00 

D6  ^  3.5 
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C  DEFINE  TRANSFORM  MATRIX  ENTRIES 

RllA  =  (C23  ♦  ((C4  *  C5  *  C6)- (S4  *  S6))-(S23  *  S5  *  C6)) 
RUB  =  ((S4  ♦  C5  ♦  C6)  +  (C4  ♦  S6)) 

T(l,l)  =  (Cl  ♦  RllA)  +  (SI  *  RUB) 

T(2,l)  =  (SI  ♦  RllA)  -  (Cl  ♦  RUB) 

T(3,l)  =  -S23  *  ((C4  *  C5  *  C6)- (S4  ♦  S6))-(C23  ♦  S5  *  C6) 
R12A  =  -  C23  ♦  ((C4  *  C5  *  S6)+(S4  ♦  C6))+(S23  ♦  S5  *  S6) 
R12B  =  ((S4  *  C5  ♦  S6)-(C4  ♦  C6)) 

T(l,2)  =  (Cl  *  R12A)-(S1  ♦  R12B) 

T(2,2)  =  (SI  ♦  R12A)+(C1  ♦  R12B) 

T(3,2)  =  S23  ♦  ((C4  ♦  C5  *  S6)+(S4  *  C6))+(C23  ♦  S5  *  S6) 

R13A  =  (C23  *  C4  *  S5)+(S23  *  C5) 

R13B  =  (S4  *  S5) 

T(l,3)  =  (-C1  *  R13A)-(S1  ♦  R13B) 

T(2,3)  =  (-S1  ♦  R13A)+(C1  ♦  R13B) 

T(3,3)  =  (S23  ♦  C4  ♦  S5)- (C23  *  C5) 

R14A  =  (-17.24  *  S23)+(17.38  ♦  Cr' 

R14B  =  (D2  +  D3) 

T(l,4)  =  (Cl  *  R14A)-(S1  ♦  R14B) 

T(2,4)  =  (SI  *  R14A)+(C1  ♦  R14B) 

T(3,4)  =  (-17.24  ♦  C23)- (17.38  *  S2) 

C  DECIDE  ON  DATA  TO  BE  REPORTED  TO  TIP  OR  VRIST  PIN 


TIP  =  0 

PRINT  DO  YOU  WANT  POSITION  TO  BE  REPORTED  TO  TIP  ’ 
PRINT  ♦ OF  ROBOT  ARM  (1)  OR  VRIST  PIN  (0)  ?  ’ 

READ (5,^)  TIP 

IF  POSITION  DATA  IS  TO  BE  REPORTED  V.R.T.  TIP  OF  ROBOT  ARM 
ADD  D6  *  APPROACH  VECTOR  TO  POSITION  VECTOR  I.E. 


T(R0V,4)  =  T(R0V.4)  +  (T(R0V,3)  ♦  D6) 
IF  (TIP  .EQ.  1)  THEN 


C 

C 

C 


T(3,4] 

ENDIF 
RETURN 
END 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
C  OUTPUT  TO  SCREEN 

SUBROUTINE  TOUT(T) 

REAL  T(4,4) 

INTEGER  I,J 
C  OPEN  DATA  FILE 


OPEN (UNIT=6 , STATUS= ’ NEV  %  FILE= ’ LDKIN . OUT ’ ) 
PRINT  ’ 


DO  601  I  =  1,4 
C  WRITE  TO  SCREEN 

VRITE(5,*)  (T(I,J2,J=1,4) 

C  VRITE  TO  OUTPUT  FILE  LDKIN. OUT 
VRITE(6,*)  (T(I,J),J=1,4) 

601  CONTINUE 

mm  ’ 

RETURN 

END 

ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 
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DIRECT  KINEIATICS  SIKULiTION  FOR  MERLIN  6500  RIGHT  ARM 


ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 
C  MERLIN  ROBOT  RIGHT  ARM  KINEMATICS  SIMULATION  PROGRAM 
ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 
C  MAIN  PROGRAM 

C  DEFINE  REAL  R  INTEGER  VARIABLES 
INTEGER  INTRO 

REAL  T(4,4),V,S,E,VR,VP,HR 
C  DESCRIBE  THE  hOGRAM 

PRINT  THIS  PROGRAM  PERFORMS  MATHEMATICAL  SIMULATION  OF’ 

PRINT  THE  KINEMATICS  OF  THE  MERLIN  6500  RIGHT  ARM  ROBOT.’ 

10  PRINT  ’ 

PRINT  DO  YOU  NEED  AN  INTRODUCTION  TO  THE  PROGRAM  ?  ’ 

PRINT  YES  - >  1.’ 

PRINT  NO  ---->  2.’ 

INTRO  =  2 
READ(5,*)  INTRO 
IF (INTRO  .EQ.  1)  THEN 
CALL  INTROD 

ELSE  IF (INTRO  .EQ.  2)  THEN 
GOTO  11 
ELSE 

PRINT  ENTRY  ERROR  ’ 

GOTO  10 
ENDIF 

C  FIND  THE  USER- DEFINED  ANGLES 

11  CALL  ANGLES(V,S,E,VR,VP,HR) 

PRINT  ’ 

CALL  DIRKIN(V,S,E,VR,VP,HR,T) 

CALL  TODT(T) 

STOP 

END 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
C  INTRODUCTION  TO  THE  PROGRAMME 
SUBROUTINE  INTROD 


PRINT  * 
PRINT  * 
PRINT  ♦ 
PRINT  ♦ 
PRINT  * 
PRINT  * 
PRINT  * 
PRINT  * 
PRINT  * 
PRINT  * 
PRINT  * 
PRINT  * 
PRINT  * 
PRINT  * 
PRINT  * 


THE  PROGRAM  REQUESTS  THE  USER  TO  ENTER  THE  JOINT 
ANGLES  FOR  EACH  OF  THE  FOLLOWING  JOINTS  ’ 
JOINT  RANGE  ’ 


’  WAIST  JOINT 
’  SHOULDER  JOINT 
’  ELBOW  JOINT 
’  WRIST  ROLL 
’  WRIST  PITCH 
’  TOOL  ROLL 


RANGE  +  147  TO 
RANGE  +  56  TO 
RANGE  +  56  TO 
RANGE  +  360  TO 
RANGE  +  90  TO 
RANGE  +  360  TO 


147  DEGREES 
236  DEGREES 
236  DEGREES 
360  DEGREES 
90  DEGREES 
360  DEGREES 


’  THE  PROGRAM  RETURNS  THE  FINAL  TRANSFORMATION  ’ 

’  MATRIX  i.e.  THE  POSITION  k  ORIENTATION  MATRIX  ’ 
’  DEFINED  AT  THE  WRIST  PIN  OR  TIP  OF  THE  ARM.  ’ 


RETURN 


END 
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ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

C  ENTRY  OF  JOINT  ANGLES  BY  THE  USEE 
SUBROUTINE  ANGLES (V , S , E , VR , VP , HR) 

REAL  Vl,Sl,El,VRl,VPl,HRi,V,S,E,VR,VP,HR,PI 
C  DEFINE  PI 

PI  =  3.141592654 
C  VAIST 

101  V  =  0.0 
VI  =  0.0 

PRINT  * ENTER  VAIST  ANGLE  (  +/-  147  DEGREES  )  ==>  ’ 

READ^S,-^)  VI 

IF  (ABS(Vl)  .GT.  147.0)  THEN 
PRINT  ERROR  --  RANGE  IS  +/-  147  DEGREES.  ’ 

GOTO  101 
ENDIF 

V  =  VI  *  PI  /  180.0 
C  SHOULDER 

102  S  =  0.0 
SI  =  0.0 

PRINT  ♦ ENTER  SHOULDER  ANGLE  (  56  TO  -236  DEGREES  )  ==>  ’ 
READ(5,*)  SI 

IF  ((SI  .GT.  56.0)  .OR.  (SI  .LT.  -236.0))  THEN 
PRINT  ERROR  --  RANGE  IS  56  TO  -236  DEGREES.  ’ 

GOTO  102 
ENDIF 

S  =  SI  *  PI  /  180.0 
C  ELBOV 

103  E  =  0.0 
El  =  0.0 

PRINT  ♦ ENTER  ELBOV  ANGLE  (  56  to  -236  DEGREES  )  ==>  ’ 
READ(5,^)  El 

IF  ((El  .GT.  56.0)  .OR.  (El  .LT.  -236.0))THEN 
PRINT  ERROR  --  RANGE  IS  56  TO  -236  DEGREES.  ’ 

GOTO  103 
ENDIF 

E  =  El  *  PI  /  180.0 
C  VRIST  ROLL 

104  VR  =  0.0 
VRl  =  0.0 

PRINT  * ENTER  VRIST  ROLL  (  +/-  360  DEGREES  )  ==>  ’ 

READ(5,*)  VRl 

IF  (ABS(VRl)  .GT.  360.0)  THEN 
PRINT  ERROR  --  RA^E  IS  +/-  360  DEGREES.  ’ 

GOTO  104 
ENDIF 

VR  =  VRl  *  PI  /  180.0 
C  VRIST  PITCH 

105  VP  =  0.0 
VPl  =  0.0 

PRINT  * ENTER  VRIST  PITCH  (  +/-  90  DEGREES)  ==>  ’ 

READ (5,^)  VPl 

IF  (ABS((fPl)  .GT.  90.0)  THEN 
PRINT  ERROR  --  RANGE  IS  +/-  90  DEGREES.  ’ 

GOTO  105 
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ENDIF 

VP  =  VPl  ♦  PI  /  180.0 
C  HAND  ROLL 
106  HR  =  0.0 
HRl  =  0.0 

PRINT  ♦  ’  ENTER  HAND  ROLL  (  +/-  360  DEGREES  )  ==>  ’ 

READ(5,^)  HRl 

IF(ABS(HR1)  .GT.  360.0)  THEN 
PRINT  ERROR  --  RANGE  IS  +/-  360  DEGREES.  ’ 

GOTO  106 
ENDIF 

HR  =  HRl  *  PI  /  180.0 

RETURN 

END 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

C  KINEMATICS  IMPLEMENTATION 

SUBROUTINE  DIRKIN(V,S,E,VR,VP,HR,T) 

INTEGER  I,J,TIP 

REAL  T(4,4),C1,C2,C3,C4,C5,C6,S1,S2,S3,S4,S5,S6,C23,S23,D2,D3, 
$R11A,R11B,R12A,R12B,R13A,R13B,R14A,R14B,V,S,E,VR,VP,HR,D6 
C  INITIALIZE  MATRIX 
DO  301  I  =  1,4 
DO  301  J  =  1,4 
T(I,J)  =  0.0 
301  CONTINUE 

T(4,4)  =  1.0 

C  DEFINE  COSINES  AND  SINES 
Cl  =  COS(V) 

C2  =  COS(S) 

C3  =  COS  E) 

C4  =  cos(vi) 

C5  =  COS(VP) 

C6  =  COS(HR) 

51  =  SIN(V) 

52  =  SIN(S) 

53  =  SIN(E) 

54  =  SIN(VR) 

55  =  SIN(VP) 

56  =  SIN(HR) 

C23  =  COS(S  +  E) 

S23  =  SIN(S  +  E) 

C  DEFINE  D2,  D3  k  D6(  APPROXIMATELY  )  FOR  THE  RIGHT  ARM. 

C  D2  IS  THE  SIGNED  DISTANCE  FROM  THE  GLOBAL  X  AXIS  TO  THE  SHOULDER 
C  CENTRAL  AXIS  (  ALIGNED  ALONG  THE  UPPER  ARM  ) .  D3  IS  THE  SIGNED 
C  DISTANCE  FROM  THE  SHOULDER  CENTRAL  AXIS  TO  THE  ELBOV  CENTRAL 
C  AXIS,  ALIGNED  WITH  THE  CENTER  OF  THE  LOVER  ARM.  THESE  VALDES 
C  (D2  ft  D3)  CHANGE  SIGN  FOR  THE  LEFT  ARM.  D6  IS  THE  SIGNED 
C  DISTANCE  FROM  THE  VRIST  PIN  TO  THE  TIP  OF  THE  ROBOT  ARM. 

C  (  REFER  TO  THE  ARM  KINEMATICS  FOR  MORE  DETAILS.  ) 

D2  =  -  19.00 
D3  =  7.00 
D6  =  3.5 

C  DEFINE  TRANSFORM  MATRIX  ENTRIES 

RllA  =  (C23  *  ((C4  *  C5  *  C6)- (S4  *  S6))-(S23  *  S5  *  C6)) 
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KllB  = 


=  Cl 


=  (SI 


♦  C5  *  C6) 
♦  illA) 
RllA) 


;  + 

+  (SI  * 

-  (Cl  ♦ 
♦  C5  ♦  ( 


I) 


(C4  ♦  S6 
RUB 
RUB 

=  -S23  ♦  ((C4  ♦  C5  *  C6)-(S4  ♦  S6))-( 
-  C23  *  ((C4  *  C5  ♦  S6)+(S4  ♦  C6))+(S 
((S4  ♦  C5  *  S6)-(C4  ♦  C6)) 

^  R12A)-(S1  ♦  R12B) 

+(C1  ♦  R12B) 

♦  C5  ♦  S6)+(S4 
♦  S5)+(S23  ♦  C5) 


- (C23  ♦  S5  * 
23  ♦  S5  * 


C6) 

S6) 


T(l,3 
T(2,3 
T(3,3 
R14A  = 
R14B 
T(l,4 
t(2,4 
T(3,4 
DECIDE  0 
TIP  = 
PRINT 
PRINT 


C6))+(C23  ♦  S5  ♦  S6) 


=  (-C1  *'R13A)-(S1  ♦  R13B) 
=  (-S1  *  R13A)+(C1  ♦  R13B) 


1 


=  (S23  ♦  C4  *'S5)-(C23  ♦  CS) 
^  17.24  ♦  S23)+(17.38  ♦  C2) 

+  D3) 


D2 
=  (Cl 


R14A)-( 

R14A)+( 


SI 

Cl 


R14B) 

R14B) 


-  (  1' 

DATA  TO  BE  REPOh^D  TO  TIP  OR  VRIST  PIN 


=  (- 1^24  *  C23 j-  (17.38  *  S2^ 


C 

C 

C 


DO  YOD  VANT  POSITION  TO  BE  REPORTED  TO  TIP  ’ 
OF  ROBOT  ARM  (1)  OR  VRIST  PIN  (0)  ?  ’ 

READ (5,*)  TIP 

IF  POSITION  DATA  IS  TO  BE  REPORTED  V.R.T.  TIP  OF  ROBOT  ARM 
ADD  D6  *  APPROACH  VECTOR  TO  POSITION  VECTOR  I.E. 

T(R0V,4)  =  T(R0V,4)  +  (T(R0V,3)  ♦  D6) 

IF (TIP  .EQ.  1)  THEN 

T(l,3] 


* 

♦ 


D6 

D6 

D6 


RETURN 

END 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

C  OUTPUT  TO  SCREEN 

SUBROUTINE  TOUT(T) 

REAL  T(4,4) 

INTEGER  I,J 
C  OPEN  DATA  FILE 

OPEN (DNIT=6 , STATUS= ’ NEV ’ , FILE= ’ LDKIN . OUT ’ ) 

PRINT  ’ 

DO  601  I  =  1,4 
C  WRITE  TO  SCREEN 

VRITEfS,*)  (T(I,J),J=1,4) 

C  WRITE  TO  OUTPUT  FILE  LDKIN. OUT 
VRITE(6,*)  (T(I,J),J=1,4) 

601  CONTINUE 

PRINT  ’ 

RETURN 

END 

ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 
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APPENDIX  ■ 

lE&LIN  6500  MANIPULATOR  INVERSE  KINEMATICS  SIMULAxION  -  FORTRAN  CODE 

-  LEFT  ARM 

ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

MERLIN  LEFT  SHOULDERED  ROBOT  INVERSE  KINEMATICS  PROGRAM 
PR06RAOED  BY  RANVIR  S.  SOLANKI 

VRI6HT  STATE  UNIVERSITY 
DAYTON,  OH  -  45435 
CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
C  MAIN  PROGRAM 

C  DEFINE  REAL  k  INTEGER  VALUES 
INTEGER  FLAG, REST ART, VSP ACE 

REAL  T  ( 4 , 4 ) ,  Z  ( 4 , 7 ) ,  T1 ,  T2P 1 ,  T2P2 ,  T3P ,  T3N ,  T4A ,  T4B ,  T5A ,  T5B , 
$T6A,T6B,A2,D2,D3,D4,D6,PI,VP,VN,S1,S2,EP,EN,VR1,VR2, 

$ VR3 , VR4 , VP 1 , VP2 , VP3 , VP4 , HRl , HR2 , HR3 , HR4 , DUM , TPP , TPN 
PRINT  MERLIN  6500  LEFT  SHOULDER  ARM  ’ 

PRINT  INVERSE  KINEMATICS  SIMULATION  ’ 

1  PRINT  ’ 

C  DEFINE  VALUE  OF  CONSTANTS 
PI  =  3.141592653589792 

C  SETUP  KINEMATIC  PARAMETERS  FOR  THE  MERLIN  6500  50  LB.  ROBOT 
C  A2  IS  THE  DISTANCE  BETWEEN  SHOULDER  JOINT  AND  ELBOV  JOINT 
A2  =  17.38 

C  D4  IS  THE  DISTANCE  FROM  ELBOW  JOINT  TO  WRIST  PIN 
D4  =  17.24 

C  D6  IS  THE  DISTANCE  FROM  WRIST  PIN  TO  TIP  OF  THE  END- EFFECTOR 
D6  =  3.5 

C  SET  UP  D2  AND  D3.  D2  IS  THE  DISTANCE  FROM  THE  WAIST  VERTICAL 
C  AXIS  TO  THE  CENTER  OF  THE  UPPER  ARM.  D3  IS  THE  DISTANCE  FROM 
C  THE  CENTER  OF  THE  UPPER  ARM  TO  THE  CENTER  OF  THE  LOVER  ARM. 

C  FOR  LEFT  HAND,  D2  AND  D3  ARE 
D2  =  19.00 
D3  =  -7.00 

C  INITIALIZE  ALL  GLOBAL  VARIABLES  (  RETURNED  VARIABLES  ARE 
C  INITIALIZED  INSIDE  THE  SUBHniJTINE  ONLY  ) 

VP  =  0.0 

51  -  0.0 

52  =  0.0 
EP  =  0.0 
EN  =  0.0 
VRl  =  0.0 
VR2  =  0.0 
VR3  =  0.0 
VR4  =  0.0 
VPl  =  0.0 
VP2  =  0.0 
VP3  =  0.0 
VP4  =  0.0 
HRl  =  0.0 
HR2  =  0.0 
HR3  =  0.0 
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HR4  =  0.0 
DUM  =  0.0 
T2P1  =  0.0 
T2N1  =  0.0 
T2P2  =  0.0 
T2N2  =  0.0 

C  INITIALIZE  [  Z  1  MATRIX 

C  THE  FIRST  COLUMN  OF  THE  MATRIX  IS  A  FLAG  FOR  VALIDITY  OF  THE 
C  SET  OF  JOINT  ANGLES  BEING  ALL  VITHIN  THEIR  RANGES.  THE  REMAINING 
C  4  X  6  MATRIX  IS  USED  TO  STORE  THE  RESULTS  OF  THE  COMPUTATIONS 
C  IN  THE  ORDER  VAIST,  SHOULDER,  ELBOV,  VRIST  ROLL,  VRIST  PITCH, 

C  AND  HAND  ROLL. 

DO  2  I  =  1,4 
DO  2  J  =  1,7 
Z(I,J)  =  0.0 

2  CONTINUE 

C  ENTER  POSITION  AND  ORIENTATION  MATRIX  FROM  DATAFILE  OR  SCREEN 

3  CALL  MATENTER(T,D6) 

C  FLAG  SET  UP  FOR  END  POSITION  IN/OUT  OF  WORKSPACE. 

C  VSPACE  =  0  IF  THE  END- EFFECTOR  IS  INSIDE  THE  WORKSPACE 
C  WSPACE  =  1  IP  THE  END- EFFECTOR  IS  OUTSIDE  THE  WORKSPACE 
C  SET  DEFAULT  WSPACE  FLAG  =  0 
WSPACE  =  0 

C  COMPUTE  WAIST  ANGLES  T1 

C  IN  THE  CALL  STATEMENT  BELOW,  T  IS  THE  4X4  POSITION  AND 
C  ORIENTATION  WORKSPACE,  T1  IS  THE  COMPUTED  WAIST  ANGLE. 

CALL  WAIST (T,T1,D2,D3, WSPACE) 

C  IF  POSITION  DESIRED  AS  END- POINT  IS  OUTSIDE  THE  WORKSPACE, 

C  GET  A  NEW  SET  OF  ENDPOINTS  FROM  THE  USER. 

IF (WSPACE  .EQ.  1)  THEN 
GOTO  3 
ENDIF 

C  CONVERT  WAIST  ANGLE  FROM  RADIANS  TO  DEGREES 
C  A  DUMMY  VARIABLE  (DUM)  IS  USED  HERE  SINCE  WE  ARE  DEALING 
C  WITH  ONE  WAIST  ANGLE  ONLY. 

CALL  RADEG(T1,0.0,WP,DUM) 

C  STORE  RESULTS  OF  WAIST  IN  [Z]  MATRIX  (SECOND  COLUMN) 

DO  5  I  =  1,4 
Z(I,2)  =  WP 
5  CONTINUE 

C  RESET  THE  WSPACE  FLAG  TO  0  FOR  ELBOW  ANGLE  COMPUTATIONS. 

WSPACE  =  0 

C  COMPUTE  ELBOW  ANGLES  T3P,T3N. 

CALL  ELBOW(T , T3P , T3N , A2 , D2 , D3 , D4 , WSPACE) 

C  IF  USER  DEFINED  END- POSITION  IS  OUTSIDE  THE  WORKSPACE, 

C  RE-ENTRY  OF  MATRIX  BY  THE  USER. 

IF (WSPACE  .Eq.  1)  THEN 
GOTO  3 
ENDIF 

C  CONVERT  ELBOW  ANGLES  FROM  RADIANS  TO  DEGREES 
CALL  RADEG(T3P,T3N,EP,EN) 

C  STORE  RESULTS  OF  ELBOW  ANGLE  SOLUTION  IN  THE  FOURTH  COLUMN 
C  OF  MATRIX  [Z] 

DO  6  I  =  1,2 
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Z(I,4)  =  EP 
Z(I+2,4)  =  EN 

6  CONTINUE 

C  COMPOTE  (  SHOULDER  +  ELBOV  )  ANGLES  TPP,TPN 
CALL  SHOULDER (T , A2 , D4 , T1 , T3P , T3N , TPP , TPN ) 

C  COMPUTE  SHOULDER  ANGLES  T2P1,T2P2 
T2P1  =  TPP  -  T3P 
T2P2  =  TPN  -  T3N 

C  CONVERT  SHOULDER  ANGLES  FROM  RADIANS  TO  DEGREES 
CALL  RADEG(T2Pl,T2P2,Si,S2) 

C  STORE  RESULTS  OF  SHOULDER  ANGLES  IN  [Z]  MATRIX  (THIRD  COLUMN) 
DO  7  I  =  1,2 
Z(I,3)  =  SI 
Z(I+2,3)  -  S2 

7  CONTINUE 

C  COMPUTE  VRIST  ROLL  ANGLES 
FLAG  =  0 

CALL  VR0LL(T,T4P1,T4P2,T1,TPP,TPN) 

C  CONVERT  VRIST  ROLL  ANGLES  FROM  RADIANS  TO  DEGREES 
CALL  RADEG(T4P1,T4P2,VR1,VR2) 

C  COMPUTE  ’VRIST  FLIPPED’  SOLUTIONS 
VR3  =  VRl  +  180.0 
VR4  =  VR2  +  180.0 

C  STORE  RESULTS  OF  VRIST  ROLL  IN  [Z]  MATRIX  (FIFTH  COLUMN) 
Z(l,5)  =  VRl 
Z(2,5)  =  VR3 
Z(3,5)  =  VR2 
Z(4,5)  =  VR4 

C  COMPUTE  VRIST  PITCH  ANGLES 

CALL  VPITCH(T,T5P1,T5P2,T1,TPP,TPN,T4P1,T4P2) 

C  CONVERT  VRIST  PITCH  ANGLES  FROM  RADIANS  TO  DEGREES 
CALL  RADEG(T5P1,T5P2,VP1,VP2) 

C  COMPUTE  ’REVERSED  PITCH’  SOLUTIONS 
VP3  =  -  VPl 
VP4  =  -  VP2 

C  STORE  RESULTS  IN  [  Z  ]  MATRIX  -  SIXTH  COLUMN 
Z(l,6)  =  VPl 
Z(2,6)  =  VPS 
Z(3,6)  =  VP2 
Z(4,6)  =  VP4 
C  COMPUTE  HAND  ROLL 

CALL  HR0LL(T,T6P1,T6P2,T1,TPP,TPN,T4P1,T4P2,T5P1,T5P2) 

C  CONVERT  HAND  ROLL  ANGLES  FROM  RADIANS  TO  DEGREES 
CALL  RADEG(T6P1,T6P2,HR1,HR2) 

C  COMPUTE  ’HAND  FLIPPED’  SOLUTIONS  FOR  HAND  ROLL 
HR3  =  HRl  +  180.0 
HR4  =  HR2  +  180.0 

C  STORE  RESULTS  IN  THE  [Z]  MATRIX  (SEVENTH  COLUMN) 

Z(l,7)  =  HRl 
Z(2,7)  =  HRS 
Z(S,7)  =  HR2 
Z(4,7)  =  HR4 

C  NORMALIZE  THE  COMPUTED  RESULTS 
CALL  NORMAL(Z) 
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C  CHECK  FOR  VALIDITY  OF  EACH  SOLUTION  SET 
CALL  VALID(Z) 

C  PRINT  OUT  VALID  RESULTS  (  VALID  IF  VITHIN  JOINT  ANGLE  RANGE  ) 
PRINT  THE  VALID  INVERSE  KINEMATICS  RESULTS  ARE  ’ 

DO  51  I  =  1,4 

IF(Z(I,1)  .EQ.  0.0)  THEN 
VRITE(5,*)  ’THE  VALID  SOLUTION  NUMBER  IS 
VRITE(5,*)  (Z(I,J),J=2,7) 

ENDIF 

51  CONTINUE 

C  QUERY  FOR  RESTART 
RESTART  =  0 

PRINT  TO  RESTART  PROGRAM,  ENTER  1  ’ 

PRINT  TO  EXIT  THE  PROGRAM,  ENTER  0  ’ 

READ (5, 93)  RESTART 
IF (RESTART  .EQ.  1)  THEN 
GOTO  1 
ENDIF 

98  FORMAT (I) 

STOP 

END 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 

C  POSITION  AND  ORIENTATION  DATA  ENTRY  ROUTINE 
SUBROUTINE  MATENTER(A,D6) 

INTEGER  MATCH, TIP, DFILE 

REAL  A(4,4),PX1,PY1,PZ1,PX,PY,PZ,D6 

C  INITIALIZE  LOCAL  VARIABLES 
PXl  =  0.0 
PYl  =  0.0 
PZl  =  0.0 
PX  =  0.0 
PY  =  0.0 
PZ  =  0.0 

C  INITIALIZE  MATRIX  [A] 

100  DO  101  1-1,4 

DO  101  J  -  1,4 
A(I,J)  =  0.0 

101  CONTINUE 

C  SET  DEFAULT  TO  READ  FROM  DIRECT  KINEMATICS  DATA  FILE 
DFILE  =  1 

C  READ  FROM  DATA  FILE  ? 

PRINT  READ  TRANSFORM  MATRIX  FROM  LDKIN.OUT  ?  ’ 

PRINT  *  ’  ENTER  1  IF  YES,  2  IF  NO  ’ 

READ(5,=^)  DFILE 
IF(DFILE  .EQ.  1)  THEN 
OPEN (DNIT=6 , FILE- ’ LDKIN . OUT ’ , STATUS- ’ OLD ’ ) 

DO  105  I  -  1,4 
READ(6,*)  (A(I,J),J-1,4) 

105  CONTINUE 

CL0SE(UNIT=6) 

ELSE 

C  DATA  ENTRY  OF  POSITION  AND  ORIENTATION  MATRIX 
DO  102  I  -  1,3 
DO  102  J  -  1,4 
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PRINT  *  ’  ENTER  TRANSFORM  MATRIX  ENTRY’, I, J 
READ(5,*)  A(I,J) 

)2  CONTINUE 

ADJUST  ROV  4  ENTRIES  TO  PREVENT  ENTRY  ERROR 
A(4,l)  =  0.0 
A(4,2)  =  0.0 
A(4,3)  =  0.0 
A(4,4)  =  1.0 
ENDIP 

PRINT  OUT  MATRIX  TO  SCREEN 
PRINT  ’ 

PRINT  THIS  IS  THE  ENTERED  TRANSFORM  MATRIX.  ’ 

CALL  AODT(A) 

PRINT  IF  YOU  WANT  TO  CHANGE  THE  MATRIX,  ENTER  0  ’ 

PRINT  IF  POSITION  ENTRIES  REFER  TO  THE  TIP  OF  THE 

PRINT  END  EFFECTOR  -  ENTER  1  ’ 

PRINT  IF  POSITION  ENTRIES  ARE  VITH  RESPECT  TO  THE 

PRINT  VRIST  PIN  -  ENTER  2  ’ 

READ(5,104)  TIP 

ALLOW  CHANGE  OF  TRANSFORM  MATRIX  ENTRIES 


IF(TIP  .EQ.  0)  THEN 
GOTO  100 
ENDIF 

C  ADJUST  END  EFFECTOR  POSITION  TO  VRIST  PIN  IF  POSITION  GIVEN  IS 

C  AT  THE  TIP  OF  THE  END- EFFECTOR 
IF (TIP  .Eq.  1)  THEN 

C  SETUP  POSITION  PARAMETERS  TO  END- EFFECTOR  TIP 
PXl  =  A(l,4) 

PYl  =  A(2,4) 

PZl  =  A(3,4) 

C  ADJUST  POSITION  PARAMETERS  TO  VRIST  PIN 
PX  =  PXl  -  D6  *  A(l,3) 

PY  =  PYl  -  D6  *  A(2,3) 

PZ  =  PZl  -  D6  *  Ai3  3) 

C  RESET  POSITION  PARAMETMS  IN  [A]  MATRIX  TO  VRIST  PIN 
A(l,4)  =  PX 
A(2,4)  =  PY 
A(3,4)  =  PZ 
ENDIF 

104  FORMAT(I) 

RETURN 

END 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 

C  OUTPUT  OF  MATRIX  TO  SCREEN 
SUBROUTINE  AOUT(M) 

REAL  M(4,4) 

INTEGER  I,J 
DO  1001  I  =  1,4 
WRITERS,*)  (M(I,J),J=1,4) 

1001  CONTINUE 
RETURN 
END 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
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C  VAIST  ANGLE  COMPUTATION 

SUBROUTINE  VAIST(A,V1 ,X2,X3, SPACE) 

REAL  A(4,4),Vl,X2,X3,RflO,PX,PY,TEkl,TERM2, 

$T2,X23 

INTEGER  I, J, SPACE 

C  INITIALIZATION  OF  LOCAL  VARIABLES 
VI  =  0.0 
TERMl  =  0.0 
TERM2  =  0.0 
X23  =  0.0 
T2  =  0.0 

C  SET  UP  OF  POSITION  PARAMETERS 
PX  =  A(l,4) 

PY  =  A(2,4) 

C  COMPUTE  FIRST  TERM  FOR  VAIST  ANGLE  SOLUTION 
TERMl  =  ATAN2(PY,PX) 

C  COMPUTE  TERM2 

X23  =  (X2  +  X3) 

PXSq  =  PX  *  PX 
PYSq  =  PY  *  PY 
pxPYsq  =  pxsq  +  PYsq 
X23Sq  =  X23  *  X23 

C  USER- SPECIFIED  POSITION  INSIDE  VORKSPACE  ??? 

C  SET  FLAG  TO  INSIDE  VORKSPACE 
SPACE  =  0 

IFpXPYSq  .GT.  X23Sq)  THEN 

C  SPECIHED  POSITION  IS  INSIDE  VORK- SPACE,  SO  COMPUTE  SECOND  TERM 
GOTO  301 
ELSE 

C  USER  SPECIFIED  POSITION  IS  OUTSIDE  VORKSPACE. 

C  COMPUTE  DIFFERENCE  IN  TERMS 

ERROR  =  (ABS(PXPYSq  -  X23Sq)) 

C  IF  THE  COMPUTED  ERROR  <  0.0001,  THEN  COMPUTATIONAL  ERROR 

C  COULD  HAVE  CAUSED  THE  POSITION  TO  LIE  OUTSIDE  THE  VORKSPACE. 
IF(ERROR  .LT.  0.0001)  THEN 

C  YES,  COMPUTATIONAL  ERROR  OCCURED.  COMPUTE  T2,  FOLLOVED  BY 

C  THE  SECOND  TERM. 

T2  =  SqRT (ERROR) 

GOTO  302 
ELSE 

C  USER  SPECIFIED  POSITION  IS  DEIFINITELY  OUT  OF  VORKSPACE 
SPACE  =  1 

PRINT  OUTSIDE  VORKSPACE  ’ 

GOTO  303 
ENDIF 
ENDIF 

301  T2  =  SqRT(PXPYSq  -  X23Sq) 

302  TERM2  =  ATAN2(X23,T2) 

C  COMPUTE  SOLUTION  FOR  VAIST  ANGLE  VI 
VI  =  TERMl  -  TERM2 

303  RETURN 
END 

ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 


C  ELBOV  ANGLE  DETERMINATION  ROUTINE 

SUBROUTINE  ELBOV (A , EP , EN , B2 , 12 , 13 , 14 , SPACE) 

INTEGER  SPACE 

REAL  A(4,4),EP,EN,B2,X2,I3,X4,KA,KB,X23,T1,T2P,T2N 

C  INITIALIZE  LOCAL  VARIABLES 
EP  =  0.0 
EN  =  0.0 
KA  =  0.0 
KB  =  0.0 
T1  =  0.0 
T2P  =  0.0 
T2N  =  0.0 
X23  =  0.0 

C  SET  UP  POSITION  PARAMETERS  OF  TRANSFORM  MATRIX 
PX  =  A(l,4) 

PY  =  A(2,4) 

PZ  =  A(3,4) 

C  COMPUTE  FIRST  TERM  OF  ARCTAN  FUNCTION 
X23  =  (  X2  +  X3  ) 

KA  =  -  (PX  *  PX)  -  (PY  ♦  PY)  -  (PZ  *  PZ) 

KB  =  (B2  *  B2)  +  (X23  *  X23)  +  (X4  *  X4) 

T1  =  (KA  +  KB)  /  (  2.0  *  B2  ♦  X4) 

Tisq  =  T1  ♦  T1 

C  DETERMINE  IF  USER  DEFINED  POSITION  IS  OUTSIDE  VORKSPACE 
SPACE  =  0 

C  POSITION  IS  INSIDE  THE  VORK- SPACE  IF  TlSq  <1.0 
IF(TlSq  .LE.  1.0)  THEN 
G'^TO  401 
ELSE 

C  USER  DEFINED  POSITION  MAYBE  OUTSIDE  VORKSPACE 

C  THEREFORE,  COMPUTE  THE  ERROR 
ERROR  =  (ABS(1.0  -  TlSq)) 

C  CHECK  TO  SEE  IF  COMPUTATIONAL  ERROR  COULD  HAVE  CAUSED  THE 

C  POSITION  TO  LIE  OUTSIDE  THE  VORKSPACE 
IF (ERROR  .LT.  0.0001)  THEN 
T2P  =  SqRT(ERROR)  ' 

GOTO  402 
ELSE 

C  USER  ENTERED  POSITION  IS  OUTSIDE  VORKSPACE 
PRINT  OUTSIDE  VORKSPACE  ’ 

SPACE  =  1 
GOTO  403 
ENDIF 
ENDIF 

C  COMPUTE  SECOND  TERM  OF  ARCTAN  FUNCTION 

401  T2P  =  SqRT(1.0  -  Tisq) 

402  T2N  =  -  T2P 

C  COMPUTE  THE  TVO  POSSIBLE  SOLUTIONS  FOR  ELBOV  ANGLE  I.E.  EP  ft  EN 
EP  =  ATAN2(T1,T2P) 

EN  =  ATAN2(T1,T2N) 

403  RETURN 
END 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
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C  SHOULDER  +  ELBOV  ANGLE  DETERMINATION  ROUTINE 

SUBROUTINE  SHOULDER (A , B2 , X4 , VP , EP , EN , APP , APN) 

INTEGER  I,J 

REAL  A(4,4) ,B2,X4,VP,EP,EN,T1PP,T1PN,T2PP,T2PN,C1P,S1P 
$C3P , C3N , S3P , S3N , TIPPA ,T1PNA ,T2PPB ,T2PNB , APP , APN 
C  INITIALIZE  LOCAL  VARIABLES 
TIPP  =  0.0 
TIPN  =  0.0 
T2PP  =  0.0 
T2PN  =  0.0 
TIPPA  =  0.0 
TIPNA  =  0.0 
TIPP  =  0.0 
TIPN  =  0.0 
T2PPB  =0.0 
T2PNB  =  0.0 
APP  =  0.0 
APN  =  0.0 

C  SETUP  OF  POSITION  PARAMETERS 
PX  =  A(l,4) 

PY  =  A(2,4) 

PZ  =  A(3,4) 

C  COMPUTE  COSINE  AND  SINE  FUNCTION  VALUES  OF  THE  APPROPRIATE  ANGLES 
CIP  =  COS(VP) 

SIP  =  SIN(VP) 

C3P  =  COS(EP) 

S3P  =  SIN(EP) 

C3N  =  COS(EN) 

S3N  =  SIN(EN) 

C  COMPUTE  ALL  POSSIBLE  FIRST  TERMS  OF  ARCTAN2  FUNCTION 
C  VAIST  POSITIVE,  ELBOV  POSITIVE  (TIPP) 

TIPPA  =  02"^  C3P  *  PZ 

TIPP  =  (((B2  *  S3P)  -  X4)  *  ((CIP  *  PX)  +  (SIP  *  PY)))  -  TIPPA 
C  VAIST  POSITIVE,  ELBOV  NEGATIVE  (TIPN) 

TlPNA  =  B2  *  C3N  *  PZ 

TIPN  =  (((B2  *  S3N)  -  X4)  *  ((CIP  *  PX)  +  (SIP  *  PY)))  -  TIPNA 
C  COMPUTE  ALL  POSSIBLE  SECOND  TERMS  OF  ARCTAN2  FUNCTION 
C  VAIST  POSITIVE,  ELBOV  POSITIVE  (T2PP) 

T2PPB  =  ((B2  *  C3P)  *  ((CIP  *  PX)  +  (SIP  *  PY))) 

T2PP  =  (((B2  *  S3P)  -  X4)  *  PZ)  +  T2PPB 

C  VAIST  POSITIVE,  ELBOV  NEGATIVE  (T2PN) 

T2PNB  =  ((B2  *  C3N)  *  ((CIP  ^  PX)  +  (SIP  *  PY))) 

T2PN  =  (((B2  *  S3N)  -  X4)  ♦  PZ)  +  T2PNB 

C  COMPUTE  ALL  FOUR  POSSIBLE  SOLUTIONS  OF  (THETA  2  +  THETA  3) 

APP  =  ATAN2(T1PP,T2PP) 

APN  =  ATAN2(T1PN,T2PN) 

RETURN 

END 

cccccccccccccccccccccccccccccccccccccccrrcccccccccccccccccccccccccccc 

C  VRIST  ROLL  ANGLE  DETERMINATION  ROUTINE 

SUBROUTINE  VR0LL(A,PPP,PPN,VP,T23PP,T23PN) 

INTEGER  FPPP,FPPN 

REAL  A (4 , 4 ) , PPP , PPN , VP , T23PP , T23PN , TIP , R13 , R23 , R33 , 

$S IP , C IP , C23PP , C23PN , S23PP , S23PN , SNGCHK 
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C  INITIALIZE  LOCAL  VARIABLES 
TIP  =  0.0 
T2PPP  -  0.0 
T2PPN  -  0.0 
PPP  =  0.0 
PPN  =  0.0 

C  SET  DP  SINGULARITY  CHECK  CONDITION 
SNGCHK  =  0.005 

C  SET  FLAGS  TO  NON- SINGULAR  CASE 
FPPP  =  0 
FPPN  -  0 

C  SETUP  MATRIX  ORIENTATION  PARAMETERS 
R13  -  A(1,3) 

R23  -  A(2,3) 

R33  -  A (3, 3) 

C  SETUP  TRIG.  FUNCTIONS 
SIP  -  STN(VP) 

CIP  -  COS(VP) 

C23PP  ^  C0S(T23PP) 

S23PP  SIN(T23PP) 

C23PN  --  CQS(T23PN) 

S23PN  --  SIN(T23PN; 

C  COMPUTE  ALL  FIRST  TERMS  OF  ARCTAN2  FUNCTION 
TIP  -  (R13  *  SIP)  ^  (R23  *  CIP) 

C  COMPUTE  ALL  SECOND  TERMS  OF  ARCTAN2  FUNCTION 

T2PPP  ^  (RI3*C1P*C23PP)  -  (R23*S1P*C23PP)  +  (R33*S23PP) 

T2PPN  -  (R13*C1P*C23PN)  -  (R23*S1P*C23PN)  4  (R33*S23PN) 

C  CHECK  FOR  SINGULARITY  CONDITIONS  AT  VRIST  PITCH 

IF((T1P  -LT.  SNGCHK  .AND.  TIP  .GT.  -  SNGCHK)  .AND. 

$(T2PPP  .LT.  SNGCHK  .AND.  T2PPP  .GT.-  SNGCHK))  THEN 
FPPP  =  1 
ENDIf 

IF((T1P  .LT.  SNGCHK  .AND.  TIP  .GT.  -  SNGCHK)  .AND. 

.3fT2PPN  .LT.  SNGCHK  .AND.  T2PPN  .GT.  -  SNGCHK))  THEN 
FPPN  -  1 
ENDIF 

C  SET  VRIST  ROLL  TO  u.O  RADIANS  IF  SINGULARITY  DETECTED 

C  AT  VRIST  PITCH,  ELSE  COMPUTE  VRIST  ROLL.  NOTE  THAT  THIS  VILL 

C  CAUSE  THE  ROLL  10  SHUV  UP  ONLY  IN  HAND  ROLL  ANGLE. 

C  SOLUTION  ti  1 

IF (FPPP  .Ey.  1)  THEN 
PPP  0.0 
E 1.  S  E 

PPP  ATAN2i'TlP.T2PPP) 

ENDIF 

C  SOLUTION  t:  2 

IF (FPPN  .EQ.  1)  THEN 
PPN  -  0.0 
ELSE 

PPN  ATAN2(TIP,T2PPN; 

F.VDLF 

RETURN 

END 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 


C  VRIST  PITCH  DETERMINATION 

SUBROUTINE  VPITCH ( A , A5P 1 , A5P2 , VP , APP , APN , V4P 1 , V4P2 ) 

REAL  A(4,4),A5P1,A5P2,VP,APP,APN,V4P1,V4P2, 

$T5A1PPPP , T5A2PPPP , T5A3PPPP ,T6APPPP , T5A1PPNP , T5A2PPNP , T5A3PPNP , 
$T5APPNP , T5B IPPPP , T5B2PPPP , T5B3PPPP , T5BPPPP , T5B IPPNP , T5B2PPNP , 
$T5B3PPNP ,  T5BPPNP ,  R13 ,  R23 ,  R33 ,  ClP ,  S  IP ,  C23PP ,  S23PP , 

$C23PN , S23PN , C4P 1 , S4P 1 , C4P2 , S4P2 

C  INITIALIZE  LOCAL  VARIABLES  TO  0.0 
T5A1PPPP  =0.0 
T5A2PPPP  =0.0 
T5A3PPPP  =0.0 
T5APPPP  =0.0 
T5A1PPNP  =0.0 
T5A2PPNP  =0.0 
T5A3PPNP  =0.0 
T5APPNP  =0.0 
T5B1PPPP  =0.0 
T5B2PPPP  =0.0 
T5B3PPPP  =0.0 
T5BPPPP  =0.0 
T5B1PPNP  =0.0 
T5B2PPNP  =0.0 
T5B3PPNP  =0.0 
T5BPPNP  =0.0 
A5P1  =  0.0 
A5P2  =  0.0 

C  SETUP  ORIENTATION  PARAMETERS 
R13  =  A(l,3) 

R23  =  A(2,3 
R33  =  A(3,3) 

C  SETUP  TRIG.  FUNCTIONS 
ClP  =  COS(VP) 

SIP  =  SIN(VP) 

C23PP  =  COS (APP) 

S23PP  =  SIN(APP) 

C23PN  =  COS(APN) 

S23PN  =  SIN(APN) 

C4P1  =  C0S(V4P1) 

S4P1  =  SIN(V4P1) 

C4P2  =  C0S(V4P2) 

S4P2  =  SIN(V4P2) 

C  COMPUTE  FIRST  TERMS  OF  THE  ARCTAN2  FUNCTIONS 

T5A1PPPP  =  -  (R13  *  aClP  *  C23PP  *  C4P1)  +  (SIP  *  S4P1))) 

T5A2PPPP  =  -  (R23  *  ((SIP  *  C23PP  *  C4Pl)  -  (ClP  *  S4P1))) 

T5A3PPPP  =  R33  *  S23PP  *  C4P1 

T5APPPP  =  T5A1PPPP  +  T5A2PPPP  +  T5A3PPPP 

T5A1PPNP  =  -  (R13  *  ((ClP  *  C23PN  *  C4P2)  +  (SIP  *  S4P2))) 

T5A2PPNP  =  -  (R23  *  ((SIP  *  C23PN  *  C4P2)  -  (ClP  ♦  S4P2))) 

T5A3PPNP  =  R33  *  S23PN  *  C4P2 

T5APPNP  =  T5A1PPNP  +  T5A2PPNP  +  T5A3PPNP 

C  COMPUTE  SECOND  TERMS  OF  THE  ARCTAN2  FUNCTIONS 
T5B1PPPP  =  -  (ClP  *  S23PP  *  R13) 

T5B2PPPP  =  -  (SIP  *  S23PP  ♦  R23) 

T5B3PPPP  =  -  (C23PP  *  R33) 
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T5BPPPP  =  T5B1PPPP  +  T5B2PPPP  +  T5B3PPPP 
T5B1PPNP  =  -  (CIP  ♦  S23PN  *  R13) 

T5B2PPNP  =  -  (SIP  *  S23PN  ♦  123) 

T5B3PPHP  =  -  (C23PN  ♦  R33) 

T5BPPNP  =  T5B1PPNP  +  T5B2PPNP  +  T5B3PPNP 
C  COlPUTE  VRIST  PITCH  ANGIES  USING  ARCTAN2  FUNCTION 
A5P1  =  ATAN2(T5APPPP,T5BPPPP) 

A5P2  =  ATAN2(T5APPNP,T5BPPNP) 

RETURN 

END 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
C  DETERIINATION  OF  HAND  ROLL  ANGLES 

SUBROUTINE  HR0LL(A,A6P1,A6P2,VP,APP,APN,A4P1,A4P2,A5P1,A5P2) 
INTEGER  I,J 

REAL  A (4 , 4) , A6P1 , A6P2 , VP , APP , APN , A4P1 , A4P2 , A5P1 , A5P2 , 

SPPPPPAl , PPPPPA2 , PPPPPA3 ,PPPPPA , PPNPPAl ,PPNPPA2 , PPNPPA3 ,PPNPPA 
SPPPPPBl , PPPPPB2 , PPPPPB3 ,PPPPPB ,PPNPPB1 , PPNPPB2 , PPNPPB3 , PPNPPB 
C  INITIALIZE  LOCAL  VARIABLES  TO  0.0 


PPPPPAl 

0.0 

PPPPPA2 

0.0 

PPPPPA3 

= 

0.0 

PPPPPA 

zz 

0.0 

PPNPPAl 

= 

0.0 

PPNPPA2 

0.0 

PPNPPA3 

— 

0.0 

PPNPPA 

0.0 

PPPPPBl 

z: 

0.0 

PPPPPB2 

= 

0.0 

PPPPPB3 

0.0 

PPPPPB 

£ 

0.0 

PPNPPB 1 

Z 

0.0 

PPNPPB2 

r 

0.0 

PPNPPB3 

r 

0.0 

PPNPPB 

=: 

0.0 

C  INITIALIZE  VRIST  ROLL  ANGLES  TO  0.0 
A6P1  =  0.0 
A6P2  =  0.0 

C  SETUP  ROTATION  PARAMETERS 
Rll  =  A(l,l) 

R21  =  A(2,l) 

R31  =  A(3,l) 

C  SETUP  UP  TRIG.  FUNCTIONS 
CIP  =  COS (VP) 

SIP  =  SIN(VP) 

C23PP  =  COS(APP) 

S23PP  =  SIN(APP) 

C23PN  =  COS (APN) 

S23PN  =  SIN(APN) 

C4P1  =  C0S(A4P1) 

S4P1  =  SIN(A4P1) 

C4P2  =  C0S(A4P2) 

S4P2  =  SIN(A4P2) 

C5P1  =  C0S(A5P1) 

S5P1  =  SIN(A5P1) 
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C5P2  =  C0S(A5P2) 

S5P2  =  SIN(A5P2) 

C  COKPUTE  THE  FIRST  TEEMS  FOR  THE  ARCTAN2  FUNCTION 

PPPPPAl  =  Rll  *  ((CIP  ♦  C23PP  *  S4P1)  -  (SIP  *  C4P1)) 

PPPPPA2  =  R21  ♦  ((SIP  ♦  C23PP  ♦  S4P1)  +  (CIP  *  C4P1)) 

PPPPPA3  =  R31  *  (S23PP  ♦  S4P1) 

PPPPPA  =  -  PPPPPAl  -  PPPPPA2  +  PPPPPA3 

PPNPPAl  =  Rll  ♦  ((CIP  *  C23PN  *  S4P2)  -  (SIP  ♦  C4P2)) 

PPNPPA2  =  R21  *  ((SIP  *  C23PN  *  S4P2)  +  (CIP  *  C4P2)) 

PPNPPA3  =  R31  ♦  (S23PN  *  S4P2) 

PPNPPA  =  -  PPNPPAl  -  PPNPPA2  +  PPNPPA3 
C  COMPUTE  THE  SECOND  TERMS  FOR  THE  ARCTAN2  FUNCTION 

PPPPPBl  =  Rll  ♦  (C5P1  *  ((CIP  ♦  C23PP  *  C4P1)  +  (SIP  *  S4P1)) 

$  -  (CIP  *  S23PP  ^  S5P1)) 

PPPPPB2  =  R21  ♦  (C5P1  *  ((SIP  *  C23PP  ♦  C4P1)  -  (CIP  *  S4P1)) 

$  -  (SIP  ♦  S23PP  *  S5P1)) 

PPPPPB3  =  R31  *  ((S23PP  ♦  C4P1  ♦  C5P1)  +  (C23PP  *  S5P1)) 

PPPPPB  =  PPPPPBl  +  PPPPPB2  -  PPPPPB3 

PPNPPBl  =  Rll  *  (C5P2  *  ((CIP  ♦  C23PN  *  C4P2)  +  (SIP  ♦  S4P2)) 

$  -  (CIP  *  S23PN  *  S5P2)) 

PPNPPB2  =  R21  *  (C5P2  *  ((SIP  ♦  C23PN  *  C4P2)  -  (CIP  *  S4P2)) 

$  -  (SIP  *  S23PN  ^  S5P2)) 

PPNPPB3  =  R31  *  ((S23PN  *  C4P2  ♦  C5P2)  +  (C23PN  ♦  S5P2)) 

PPNPPB  =  PPNPPBl  +  PPNPPB2  -  PPNPPB3 
C  COMPUTE  THE  HAND  ROLL  ANGLE  USING  THE  ARCTAN2  FUNCTION 
A6P1  =  ATAN2 (PPPPPA, PPPPPB) 

A6P2  =  ATAN2 (PPNPPA, PPNPPB) 

RETURN 

END 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
C  RADIAN  TO  DEGREE  CONVERSION  ROUTINE 

SUBROUTINE  RADEG (RADI, RAD2 , DEGl , DEG2 ) 

REAL  RADI, RAD2, DEGl, DEG2, PI 
C  INITIALIZE  LOCAL  VARIABLES  AND  RETURNED  VALUES 
DEGl  =  0.0 

DEG2  =  0.0 

C  DECLARE  CONSTANTS 

PI  =  3.141592653589792 

C  PERFORM  CONVERSION 

DEGl  =  RADI  *  180.0  /  PI 

DEG2  =  RAD2  *  180.0  /  PI 

RETURN 
END 

ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

C  CHECK  FOR  VALIDITY  OF  SOLUTIONS 
SUBROUTINE  VALID (A) 

REAL  A (4, 7) 

INTEGER  I,J 

C  CHECK  FOR  VALIDITY  ON  ALL  JOINTS. IF  OUT  OF  RANGE, FIRST  COLUMN=1.0 

C  NOTE  THAT  THE  RANGES  ARE  OFFSET  BY  0.01  DEGREES  TO  TAKE  CARE 
C  OF  COMPUTATIONAL  ERRORS  CAUSED  BY  THE  MACHINE. 

DO  200  I  =  1,4 

C  VAIST  RANGE  IS  FROM  +  147  TO  -147  DEGREES. 

IF(ABS(A(I,2)  .GT.  147.01)  .OR. 
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C  SHOULDER  RANGE  IS  FROM  +  56  TO  -236  DEGREES. 

$  ((A(I,3)  .GT.  56.01)  .OR.  (A(I,3)  .LT.  -236.01))  .OR. 

C  ELBOV  RANGE  IS  hOl  +  56  TO  -236  DEGREES. 

$  ((A(I,4)  .GT.  56.01)  .OR.  (A(I,4)  .LT.  -236.01))  .OR. 

C  VRIST  ROLL  IS  CONTINUOUS.  RANGE  IS  +/-  360  DEGREES. 

$  ABS(A(I,5)  .GT.  360.01)  .OR. 

C  VRIST  PITCH  RANGE  IS  FROl  +  90  TO  -90  DEGREES. 

$  ABS(A(I,6)  .GT.  90.01)  .OR. 

C  HAND  ROLL  IS  CONTINUOUS.  RANGE  IS  +[-  360  DEGREES. 

$  ABS(A(I,7)  .GT.  360.01))  THEN 

C  IF  OUT  OF  RANGE,  SET  FLAG  (COLUMN  1  OF  RESPECTIVE  ROV)  =1.0 
A(I,1)  =  1.0 
ENDIF 

200  CONTINUE 
RETURN 
END 

ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

C  NORMALIZE  THE  COMPUTED  RESULTS  SO  THAT  EACH  ANGLE  RANGES 

C  FROM  -180.0  TO  180.0  DEGREES 
SUBROUTINE  NORMAL (A) 

REAL  A (4, 7) 

INTEGER  I,J 

C  NORMALIZE  THE  ANGLES  TO  BETWEEN  - 180  AND  +180  DEGREES 
DO  701  I  =  1,4 
DO  701  J  =  2,7 
IF(A(I,J)  .GT.  180.0)  THEN 
A(I,J)  =  A(I,J)  -  360.0 
ELSEIF  (A(I,J)  .LT.  -180.0)  THEN 
A(I,J)  =  A(I,J)  +  360.0 
ENDIF 

701  CONTINUE 
RETURN 
END 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
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u  u  u  o 


MERLIN  6500  MANIPULATOR  INVERSE  KINEMATICS  SIMULATION  -  FORTRAN  CODE 

-  RIGHT  ARM 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
MERLIN  ROBOT  INVERSE  KINEMATICS  PROGRAM 
PROGRAMMED  BY  RANVIR  S.  SOLANKI 
VRIGHT  STATE  UNIVERSITY 
DAYTON,  OH  -  45435 

ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

C  MAIN  PROGRAM 

C  DEFINE  REAL  k  INTEGER  VALUES 

INTEGER  FLAG, RESTART, VSP ACE 

REAL  T (4 , 4) , Z (4 , 7) , T1 , T2P 1 , T2P2 , T3P , T3N , T4A , T4B , T5A , T5B , 
$T6A,T6B,42,D2,D3,D4,D6,PI,VP,VN,S1,S2,EP,EN,VR1,VR2, 

$ VR3 , VR4 , VP 1 , VP2 , VP3 , VP4 , HRl , HR2 , HR3 , HR4 , DUM , TPP , TPN 
PRINT  MERLIN  6500  RIGHT  SHOULDER  ARM  ’ 

PRINT  INVERSE  KINEMATICS  SIMULATION  ’ 

1  PRINT  ’ 

C  DEFINE  VALUE  OF  CONSTANTS 
PI  =  3.141592653589792 

C  SETUP  KINEMATIC  PARAMETERS  FOR  THE  MERLIN  6500  50  LB.  ROBOT 
C  A2  IS  THE  DISTANCE  BETWEEN  SHOULDER  JOINT  AND  ELBOV  JOINT 
A2  =  17.38 

C  D4  IS  THE  DISTANCE  FROM  ELBOV  JOINT  TO  WRIST  PIN 
D4  =  17.24 

C  D6  IS  THE  DISTANCE  FROM  VRIST  PIN  TO  TIP  OF  THE  END- EFFECTOR 
D6  =  3.5 

C  SET  DP  D2  AND  D3.  D2  ISTHE  DISTANCE  FROM  THE  VAIST  VERTICAL 
C  AXIS  TO  THE  CENTER  OF  THE  UPPER  ARM.  D3  IS  THE  DISTANCE  FROM 
C  THE  CENTER  OFTHE  UPPER  ARM  TO  THE  CENTER  OF  THE  LOVER  ARM. 

C  FOR  RIGHT  HAND,  D2  AND  D3  ARE 
D2  =  -19.00 
D3  =  7.00 

C  INITIALIZE  ALL  GLOBAL  VARIABLES  (  RETURNED  VARIABLES  ARE 
C  INITIALIZED  INSIDE  THE  SUBROUTINE  ONLY  ) 

VP  =  0.0 

51  =  0.0 

52  =  0.0 
EP  =  0.0 
EN  =  0.0 
VRl  =  0.0 
VR2  =  0.0 
VR3  =  0.0 
VR4  =  0.0 
VPl  =  0.0 
VP2  =  0.0 
VP3  =  0.0 
VP4  =  0.0 
HRl  =  0.0 
HR2  =  0.0 
HR3  =  0.0 
HR4  =  0.0 
DUM  =  0.0 
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T2P1  =  0.0 
T2N1  =  0.0 
T2P2  =  0.0 
T2N2  =  0.0 

C  INITIALIZE  [  Z  ]  MATRIX 

C  THE  FIRST  COLUMN  OF  THE  MATRIX  IS  A  FLAG  FOR  VALIDITY  OF  THE 
C  SET  OF  JOINT  ANGLES  BEING  ALL  VITHIN  THEIR  RANGES.  THE  REMAINING 
C  4  X  6  MATRIX  IS  USED  TO  STORE  THE  RESULTS  OF  THE  COMPUTATIONS 
C  IN  THE  ORDER  VAIST,  SHOULDER,  ELBQV,  VRIST  ROLL,  VRIST  PITCH, 

C  AND  HAND  ROLL. 

DO  2  I  =  1,4 
DO  2  J  =  1,7 
Z(I,J)  =  0.0 

2  CONTINUE 

C  ENTER  POSITION  AND  ORIENTATION  MATRIX  FROM  DATAFILE  OR  SCREEN 

3  CALL  MATENTER(T,D6) 

C  FLAG  SET  DP  FOR  END  POSITION  IN/ODT  OF  WORKSPACE. 

C  VSPACE  =  0  IF  THE  END- EFFECTOR  IS  INSIDE  THE  WORKSPACE 
C  WSPACE  =  1  IF  THE  END- EFFECTOR  IS  OUTSIDE  THE  WORKSPACE 
C  SET  DEFAULT  WSPACE  FLAG  =  0 
WSPACE  =  0 

C  COMIUTE  WAIST  ANGLES  T1 

C  IN  THE  CALL  STATEMENT  BELOW,  T  IS  THE  4X4  POSITION  AND 
C  ORIENTATION  WORKSPACE,  T1  IS  THE  COMPUTED  WAIST  ANGLE. 

CALL  WAIST (T,T1,D2,D3, WSPACE) 

C  IF  POSITION  DESIRED  AS  END- POINT  IS  OUTSIDE  THE  WORKSPACE, 

C  GET  A  NEW  SET  OF  ENDPOINTS  FROM  THE  USER. 

IF (WSPACE  .EQ.  1)  THEN 
GOTO  3 
ENDIF 

C  CONVERT  WAIST  ANGLE  FROM  RADIANS  TO  DEGREES 
C  A  DUMMY  VARIABLE  (DUM)  IS  USED  HERE  SINCE  WE  ARE  DEALING 
C  WITH  ONE  WAIST  ANGLE  ONLY. 

CALL  RADEG(T1,0.0,WP,DDM) 

C  STORE  RESULTS  OF  WAIST  IN  [Z]  MATRIX  (SECOND  COLUMN) 

DO  5  I  =  1,4 
Z(I,2)  =  WP 
5  CONTINUE 

C  RESET  THE  WSPACE  FLAG  TO  0  FOR  ELBOW  ANGLE  COMPUTATIONS. 

WSPACE  =  0 

C  COMPUTE  ELBOW  ANGLES  T3P,T3N. 

CALL  ELBOW (T , T3P , T3N , A2 , D2 ,D3 , D4 , WSPACE) 

C  IF  USER  DEFINED  END- POSITION  IS  OUTSIDE  THE  WORKSPACE, 

C  RE-ENTRY  OP  MATRIX  BY  THE  USER. 

IF (WSPACE  .EQ.  1)  THEN 

Goto  3 

ENDIF 

C  CONVERT  ELBOW  ANGLES  FROM  RADIANS  TO  DEGREES 
CALL  RADEG(T3P,T3N,EP,EN) 

C  STORE  RESULTS  OF  ELBOW  ANGLE  SOLUTION  IN  THE  FOURTH  COLUMN 
C  OF  MATRIX  [Z] 

DO  6  I  =  1,2 
Z(I,4)  =  EP 
Z(l+2,4)  =  EN 
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6  CONTINUE 

C  COMPUTE  (  SHOULDER  +  ELBOV  )  ANGIES  TPP,TPN 
CALL  SHOULDER(T , A2 , D4 ,T1 ,T3P ,T3N ,TPP , TPN) 

C  COMPUTE  SHOULDER  ANGLES  T2P1,T2P2 
T2P1  =  TPP  -  T3P 
T2P2  =  TPN  -  T3N 

C  CONVERT  SHOULDER  ANGLES  FROM  RADIANS  TO  DEGREES 
CALL  RADEG(T2P1,T2P2,S1,S2) 

C  STORE  RESULTS  OF  SHOULDER  ANGLES  IN  [Z]  MATRIX  (THIRD  COLUMN) 
DO  7  I  =  1,2 
Z(I,3)  =  SI 
Z(I+2,3)  =  S2 

7  CONTINUE 

C  COMPUTE  VRIST  ROLL  ANGLES 
FLAG  =  0 

CALL  VROLL(T,T4Pl ,T4P2 ,T1 ,TPP,TPN) 

C  CONVERT  VRIST  ROLL  ANGLES  FROM  RADIANS  TO  DEGREES 
CALL  RADEG(T4P1,T4P2,VR1,VR2) 

C  COMPUTE  ’VRIST  FLIPPED’  SOLUTIONS 
VR3  =  VRl  +  180.0 
VR4  =  VR2  +  180.0 

C  STORE  RESULTS  OF  VRIST  ROLL  IN  [Z]  MATRIX  (FIFTH  COLUMN) 
Z(l,5)  =  VRl 
Z(2,5)  =  VR3 
Z(3,5)  =  VR2 
Z(4,5)  =  VR4 

C  COMPUTE  VRIST  PITCH  ANGLES 

CALL  VPITCH (T , T5P 1 , T5P2 , T1 , TPP , TPN , T4P1 , T4P2) 

C  CONVERT  VRIST  PITCH  ANGLES  FROM  RADIANS  TO  DEGREES 
CALL  RADEG(T5P1,T5P2,VP1,VP2) 

C  COMPUTE  ’REVERSED  PITCH’  SOLUTIONS 
VP3  =  -  VPl 
VP4  =  -  VP2 

C  STORE  RESULTS  IN  [  Z  ]  MATRIX  -  SIXTH  COLUMN 
Z(l,6)  =  VPl 
Z(2,6)  =  VP3 
Z(3,6)  =  VP2 
Z(4,6)  =  VP4 
C  COMPUTE  HAND  ROLL 

CALL  HR0LL(T,T6P1,T6P2,T1,TPP,TPN,T4P1,T4P2,T5P1,T5P2) 

C  CONVERT  HAND  ROLL  ANGLES  FROM  RADIANS  TO  DEGREES 
CALL  RADEG(T6P1,T6P2,HR1,HR2) 

C  COMPUTE  ’HAND  FLIPPED’  SOLUTIONS  FOR  HAND  ROLL 
HR3  =  HRl  +  180.0 
HR4  =  HR2  +  180.0 

C  STORE  RESULTS  IN  THE  [Z]  MATRIX  (SEVENTH  COLUMN) 

Z(l,7)  =  HRl 
Z(2,7)  =  HR3 
Z(3,7)  =  HR2 
Z(4,7)  =  HR4 

C  NORMALIZE  THE  COMPUTED  RESULTS 
CALL  NORMAL (Z) 

C  CHECK  FOR  VALIDITY  OF  EACH  SOLUTION  SET 
CALL  VALID(Z) 
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C  PRINT  ODT  VALID  RESULTS  (  VALID  IF  VITHIN  JOINT  ANGLE  RANGE  ) 
PRINT  THE  VALID  INVERSE  KINEMATICS  RESULTS  ARE  :==  ’ 


DO  51  I  =  1,4 
IF(Z(I,1)  .EQ.  0.0)  THEN 
VRITE(5,*)  ’THE  miD  SOLUTION 
„J!FE(5,*)  (Z(I,J),J=2,7) 


VRITEfS,’}  ’THE  VALID  SOLUTION  NUMBER  IS 
VRITE(5,*  (Z(I,J),J=2,7) 

ENDIF 

51  CONTINUE 

C  QUERY  FOR  RESTART 
RESTART  =  0 

PRINT  TO  RESTART  PROGRAM,  ENTER  1  ’ 

PRINT  *,’  TO  EXIT  THE  PROGRAM,  ENTER  0  ’ 

READ (5, 98)  RESTART 
IF (RESTART  .EQ.  1)  THEN 
GOTO  1 
ENDIF 

98  FORMAT (I) 

STOP 

END 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 

C  POSITION  AND  ORIENTATION  DATA  ENTRY  ROUTINE 
SUBROUTINE  MATENTER(A,D6) 

INTEGER  MATCH, TIP, DFILE 

REAL  A(4,4),PX1,PY1,PZ1,PX,PY,PZ,D6 

C  INITIALIZE  LOCAL  VARIABLES 
PXl  =  0.0 
PYl  =  0.0 
PZl  =  0.0 
PX  =  0.0 
PY  -  0.0 
PZ  =  0.0 

C  INITIALIZE  MATRIX  [Al 

100  DO  101  I  =  1,4 

DO  101  J  =  1,4 
A(I,J)  =  0.0 

101  CONTINUE 

C  SET  DEFAULT  TO  READ  FROM  DIRECT  KINEMATICS  DATA  FILE 
DFXLE  “  1 

C  READ  FROM  DATA  FILE  ? 

PRINT  *,’  READ  TRANSFORM  MATRIX  FROM  LDKTN.OUT  ?  ’ 

PRINT  * ENTER  1  IF  YES,  2  IF  NO  ’ 

READ (5,=^)  DFILE 
IF (DFILE  .EQ.  1)  THEN 
OPEN (UNIT=6 , FILE= ’ LDKIN . ODT ’ , STATUS= ’ OLD ’ ) 

DO  105  I  =  1,4  ’ 

READ(6,*)  (A(I,J),J=1,4) 

105  CONTINUE 

CL0SE(UNIT=6) 

ELSE 

C  DATA  ENTRY  OF  POSITION  AND  ORIENTATION  MATRIX 
DO  102  I  =  1,3 
DO  102  J  =  1,4 

PRINT  *  ’  ENTER  TRANSFORM  MATRIX  ENTRY’, I, J 
READ(5,*)  A(I,J)  ’  ’ 


121 


102  CONTINUE 

C  ADJUST  ROW  4  ENTRIES  TO  PREVENT  ENTRY  ERROR 


A(4,l] 

1  =  0.0 

A(4,2 

1  =  0.0 

A(4,3 

1  =  0.0 

A(4,4] 

)  =  1.0 

ENDIF 

PRINT  OUT  MATRIX  TO  SCREEN 

PRINT  ’ 

PRINT  THIS  IS  THE  ENTERED  TRANSFORM  MATRIX.  ’ 

CALL  AOUT(A) 

PRINT  IF  YOU  WANT  TO  CHANGE  THE  MATRIX,  ENTER  0  ’ 

PRINT  IF  POSITION  ENTRIES  REFER  TO  THE  TIP  OF  THE  ’ 

PRINT  END  EFFECTOR  -  ENTER  1  ’ 

PRINT  IF  POSITION  ENTRIES  ARE  WITH  RESPECT  TO  THE  ’ 

PRINT  VRIST  PIN  -  ENTER  2  ’ 

READ (5, 104)  TIP 

C  ALLOW  FOR  CHANGE  OF  TRANSFORM  MATRIX  ENTRIES 
IF  (TIP  .EQ.  0)  THEN 
GOTO  100 
ENDIF 

C  ADJUST  END  EFFECTOR  POSITION  TO  VRIST  PIN  IF  POSITION  GIVEN  IS 

C  AT  THE  TIP  OF  THE  END- EFFECTOR 
IF (TIP  .EQ.  1)  THEN 

C  SETUP  POSITION  PARAMETERS  TO  END- EFFECTOR  TIP 
PXl  =  A(l,4) 

PYl  =  A(2,4) 

PZl  =  A(3,4) 

C  ADJUST  POSITION  HRAMETERS  TO  VRIST  PIN 
PX  =  PXl  -  D6  *  A(l,3) 

PY  =  PYl  -  D6  *  A(2,3) 

PZ  =  PZl  -  D6  *  A(3,3) 

C  RESET  POSITION  PARAMETERS  IN  [A]  MATRIX  TO  VRIST  PIN 
A(l,4)  =  PX 
A(2,4)  =  PY 
A(3,4)  =  PZ 
ENDIF 

104  FORMAT(I) 

RETURN 

END 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 

C  OUTPUT  OF  MATRIX  TO  SCREEN 
SUBROUTINE  AOUT(M) 

REAL  M(4,4) 

INTEGER  I,J 
DO  1001  I  =  1,4 
VRITE(5,*)  (M(I,J),J=1,4) 

1001  CONTINUE 
RETURN 
END 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 

C  VAIST  ANGLE  COMPUTATION 

SUBROUTINE  VAIST(A, VI ,X2,X3, SPACE) 

REAL  A(4,4),V1,X2,X3,RH0,PX,PY,TERM1,TERM2, 
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$T2,X23 

INTEGER  I, J, SPACE 

C  INITIALIZATION  OF  LOCAL  VARIABLES 
VI  =  0.0 
TERMl  =  0.0 
TERM2  =  0.0 
123  =  0.0 
T2  =  0.0 

C  SET  OP  OF  POSITION  PARAMETERS 
PI  =  A(l,4) 

PY  =  A(2,4) 

C  COMPOTE  FIRST  TERM  FOR  VAIST  ANGLE  SOLOTION 
TERMl  =  ATAN2(PY,PX) 

C  COMPOTE  TERM2 

123  =  (X2  +  X3) 

pxsq  =  PX  *  PX 
PYSq  =  PY  *  PY 
pxPYsq  =  pxsq  +  PYsq 

X23Sq  =  X23  *  X23 

C  IS  OSER- SPECIFIED  POSITION  INSIDE  THE  WORKSPACE  ? 

C  SET  FLAG  TO  INSIDE  WORKSPACE 
SPACE  =  0 

IF(PXPYSq  .GT.  X23Sq)  THEN 

C  SPECIFIED  POSITION  IS  INSIDE  WORK- SPACE,  SO  COMPOTE  SECOND  TERM 
GOTO  301 
ELSE 

C  OSER  SPECIFIED  POSITION  IS  OOTSIDE  WORKSPACE. 

C 

C  COMPOTE  DIFFERENCE  IN  TERMS 

ERROR  =  (ABS(PXPYSq  -  X23Sq)) 

C  IF  THE  COMPOTED  ERROR  <  0.0001,  THEN  COMPOTATIONAL  ERROR 
C  COOLD  HAVE  CAOSED  THE  POSITION  TO  LIE  OOTSIDE  THE  WORKSPACE. 

IF (ERROR  .LT.  0.0001)  THEN 

C  YES,  COMPOTATIONAL  ERROR  OCCORED.  COMPOTE  T2,  FOLLOWED  BY 
C  THE  SECOND  TERM. 

T2  =  SqRT(ERROR) 

GOTO  302 
ELSE 

C  USER  SPECIFIED  POSITION  IS  DEFINITELY  OUT  OF  WORKSPACE 
SPACE  =  1 

PRINT  OUTSIDE  WORKSPACE  ’ 

GOTO  303 
ENDIF 
ENDIF 

301  T2  -  SqRT(PXPYSq  -  X23Sq) 

302  TERM2  -  ATAN2(X23,T2) 

C  COMPUTE  SOLUTION  FOR  WAIST  ANGLE  VI 
VI  =  TERMl  -  TERM2 

303  RETURN 
END 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
C  ELBOW  ANGLE  DETERMINATION  ROUTINE 

SUBROUTINE  ELB0W(A,EP,EN,B2,X2,X3,X4, SPACE) 

INTEGER  SPACE 
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c 


REAL  A 
INITIALIZ 


4,4),EP,EN,B2,X2,X3,X4,KA,KB,X23,T1,T2P,T2N 
1  LOCAL  VARIABLES 


EP  =  0.0 


EN  =  0.0 
KA  =  0.0 
KB  =  0.0 
T1  =  0.0 
T2P  =  0.0 
T2N  =  0.0 


X23  =  0.0 

C  SET  UP  POSITION  PARAMETERS  OF  TRANSFORM  MATRIX 
PX  =  A(l,4) 

PY  =  A(2,4) 

PZ  =  A(3,4) 

C  COMPUTE  FIRST  TERM  OF  ARCTAN  FUNCTION 
X23  =  ^  X9  +  ^ 

KA  =  -  (PX  *  PX)  -  (PY  *  PY)  -  (PZ  *  PZ) 

KB  =  (B2  *  B2)  +  (X23  *  X23)  +  (X4  *  X4) 

T1  =  KA  +  KB)  /  2.0  *  B2  ♦  X4) 

TISQ  =  T1  *  T1 

C  DETERMINE  IF  USER  DEFINED  POSITION  IS  OUTSIDE  WORKSPACE 
SPACE  =  0 

C  POSITION  IS  INSIDE  THE  WORK- SPACE  IF  TlSQ  <1.0 
IF(TlSq  .LE.  1.0)  THEN 
GOTO  401 
ELSE 

C  USER  DEFINED  POSITION  MAYBE  OUTSIDE  WORKSPACE 

C  THEREFORE,  COMPUTE  THE  ERROR 
ERROR  =  (ABS(1.0  -  TISQ)) 

C  CHECK  TO  SEE  IF  COMPUTATIONAL  ERROR  COULD  HAVE  CAUSED  THE 

C  POSITION  TO  LIE  OUTSIDE  THE  WORKSPACE 
IF (ERROR  .LT.  0.0001)  THEN 
T2P  =  SQRT(ERROR) 

GOTO  402 
ELSE 

C  USER  ENTERED  POSITION  IS  OUTSIDE  WORKSPACE 
PRINT  OUTSIDE  WORKSPACE  ’ 

SPACE  =  1 
GOTO  403 
ENDIF 
ENDIF 

C  COMPUTE  SECOND  TERM  OF  ARCTAN  FUNCTION 

401  T2P  =  SQRT(1.0  -  TISQ) 

402  T2N  =  -  T2P 

C  COMPUTE  THE  TWO  POSSIBLE  SOLUTIONS  FOR  ELBOW  ANGLE  I.E.  EP  t  EN 
EP  =  ATAN2(T1,T2P) 

EN  =  ATAN2(T1,T2N) 

403  RETURN 
END 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 

C  SHOULDER  4  ELBOW  ANGLE  DETERMINATION  ROUTINE 

SUBROUTINE  SH0ULDER(A,B2,X4,WP,EP,EN,APP,APN) 

INTEGER  I,J  y  y  y  ) 

REAL  A(4,4),B2,X4,WP,EP,EN,T1PP,T1PN,T2PP,T2PN,C1P,S1P 
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o  o  o 


$C3P,C3N,S3P,S3N,T1PPA,T1PNA,T2PPB,T2PNB,APP,APN 
C  INITIALIZE  LOCAL  VARIABLES 
TIPP  =  0.0 
TIPN  =  0.0 
T2PP  =  0.0 
T2PN  =  0.0 
TIPPA  =  0.0 
TIPNA  =  0.0 
TIPP  =  0.0 
TIPN  =  0.0 
T2PPB  =0.0 
T2PNB  =0.0 
APP  =  0.0 
APN  =  0.0 

C  SETUP  OF  POSITION  PARAMETERS 
PX  =  A(l,4) 

PY  =  A(2,4) 

PZ  =  A(3,4) 

C  COMPUTE  COSINE  AND  SINE  FUNCTION  VALUES  OF  THE  APPROPRIATE  ANGLES 
CIP  =  COS(VP) 

SIP  =  SIN(VP) 

C3P  =  COS(EP 
S3P  =  SIN(EP) 

C3N  =  COS(EN 
S3N  =  SIN(EN) 

C  COMPUTE  ALL  POSSIBLE  FIRST  TERMS  OF  ARCTAN2  FUNCTION 
C 

C  VAIST  POSITIVE,  ELBOV  POSITIVE  (TIPP) 

TIPPA  =  B2  ^  C3P  ^  PZ 

TIPP  =  (((B2  *  S3P)  -  X4)  *  ((CIP  *  PX)  +  (SlP  *  PY)))  -  TIPPA 
C  VAIST  POSITIVE,  ELBOV  NEGATIVE  (TIPN) 

TIPNA  =  B2  *  C3N  *  PZ 

TIPN  =  (((B2  *  S3N)  -  X4)  *  ((ClP  *  PX)  +  (SIP  *  PY)))  -  TIPNA 
COMPUTE  ALL  POSSIBLE  SECOND  TERMS  OF  ARCTAN2  FUNCTION 

VAIST  POSITIVE,  ELBOV  POSITIVE  (T2PP) 

T2PPB  =  ((B2  *  C3P)  *  ((CIP  *  PX)  +  (SIP  *  PY))) 

T2PP  =  (((B2  *  S3P)  -  X4)  *  PZ)  +  T2PPB 

C  VAIST  POSITIVE,  ELBOV  NEGATIVE  (T2PN) 

T2PNB  =  ((B2  *  C3N)  *  ((CIP  *  PX)  +  (SIP  *  PY))) 

T2PN  =  (((B2  *  S3N)  -  X4)  *  PZ)  +  T2PNB 

C  COMPUTE  ALL  FOUR  POSSIBLE  SOLUTIONS  OF  (THETA  2  +  THETA  3) 

APP  =  ATAN2(T1PP,T2PP) 

APN  =  ATAN2(T1PN,T2PN) 

RETURN 

END 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
C  VRIST  ROLL  ANGLE  DETERMINATION  ROUTINE 

SUBROUTINE  VR0LL(A,PPP,PPN,VP,T23PP,T23PN) 

INTEGER  FPPP,FPPN 

REAL  A(4,4) ,PPP,PPN,VP,T23PP,T23PN,T1P,R13,R23,R33, 
$S1P,C1P,C23PP,C23PN,S23PP,S23PN,SNGCHK 
C  INITIALIZE  LOCAL  VARIABLES 
TIP  =  0.0 
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T2PPP  =0.0 
T2PPN  =0.0 
PPP  =  0.0 
PPN  =  0.0 

C  SET  UP  SINGULARITY  CHECK  CONDITION 
SNGCHK  =  0.005 

C  SET  FLAGS  TO  NON- SINGULAR  CASE 
FPPP  =  0 
FPPN  =  0 

C  SETUP  MATRIX  ORIENTATION  PARAMETERS 
R13  =  A(l,3) 

R23  =  A(2,3) 

R33  =  A(3,3) 

C  SETUP  TRIG.  FUNCTIONS 
SIP  =  SIN(VP) 

CIP  =  COS(VP) 

C23PP  =  C0S(T23PP) 

S23PP  =  SIN(T23PP) 

C23PN  =  C0S(T23PN) 

S23PN  =  SIN(T23PN) 

C  COMPOTE  ALL  FIRST  TEMS  OF  ARCTAN2  FUNCTION 
TIP  =  -  (R13  *  SIP)  +  (R23  *  CIP) 

C  COMPUTE  ALL  SECOND  TERMS  OF  ARCTAN2  FUNCTION 

T2PPP  =  - (R13*C1P*C23PP)  -  (R23*SlP*C23PP)  +  (R33*S23PP) 

T2PPN  =  -(R13*C1P*C23PN)  -  (R23*S1P*C23PN)  +  (R33*S23PN) 

C  CHECK  FOR  SINGULARITY  CONDITIONS  AT  VRIST  PITCH 

IF((T1P  .LT.  SNGCHK  .AND.  TIP  .GT.  -  SNGCHK)  .AND. 

$(T2PPP  .LT.  SNGCHK  .AND.  T2PPP  .GT.-  SNGCHK))  THEN 
FPPP  =  1 
ENDIF 

IF((T1P  .LT.  SNGCHK  .AND.  TIP  .GT.  -  SNGCHK)  .AND. 

$(T2PPN  .LT.  SNGCHK  .AND.  T2PPN  .GT.  -  SNGCHK))  THEN 
FPPN  =  1 
ENDIF 

C  SET  VRIST  ROLL  TO  0.0  RADIANS  IF  SINGULARITY  DETECTED 

C  AT  VRIST  PITCH,  ELSE  COMPUTE  VRIST  ROLL.  NOTE  THAT  THIS  VILL 

C  CAUSE  THE  ROLL  TO  SHOV  UP  ONLY  IN  HAND  ROLL  ANGLE. 

C  SOLUTION  #  1 

IF(FPPP  .Eg.  1)  THEN 
PPP  =  0.0 
ELSE 

PPP  =  ATAN2(T1P,T2PPP) 

ENDIF 

C  SOLUTION  i  2 

IF(FPPN  .EQ.  1)  THEN 
PPN  =  0.0 
ELSE 

PPN  =  ATAN2(T1P,T2PPN) 

ENDIF 

RETURN 

END 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 

C  VRIST  PITCH  DETERMINATION 

SUBROUTINE  VPITCH(A,A5P1 ,A5P2,VP, APP,APN,V4P1 ,V4P2) 
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c 


c 


c 


c 


c 


REAL  A(4,4),A5P1,A5P2,VP,APP,APN,V4P1,V4P2, 
$T5A1PPPP,T5A2PPPP,T5A3PPPP,T5APPPP,T5A1PPNP,T5A2PPNP,T5A3PPNP, 
$T5APPNP ,T5B1PPPP ,T5B2PPPP ,T5B3PPPP ,T5BPPPP ,T5B1PPNP ,T5B2PPNP , 
$T5B3PPNP,T5BPPNP,R13,R23,R33,C1P,S1P,C23PP,S23PP, 
$C23PN,S23PN,C4P1,S4P1,C4P2,S4P2 
INITIALIZE  LOCAL  VARIABLES  TO  0.0 
T5A1PPPP  =0.0 
T5A2PPPP  =0.0 
T5A3PPPP  =0.0 
T5APPPP  =0.0 
T5A1PPNP  =0.0 
T5A2PPNP  =0.0 
T5A3PPNP  =0.0 
T5APPNP  =0.0 
T5B1PPPP  =  0.0 
T5B2PPPP  =0.0 
T5B3PPPP  =0.0 
T5BPPPP  =0.0 
T5B1PPNP  =  0.0 
T5B2PPNP  =0.0 
T5B3PPNP  =0.0 
T5BPPNP  =  0.0 
A5P1  =  0.0 


A5P2  =  0.0 

SETUP  ORIENTATION  PARAMETERS 
R13  =  A(l,3) 

R23  =  A(2,3 
R33  =  A(3,3) 

SETUP  TRIG.  FUNCTIONS 
CIP  =  COS(VP) 

SIP  =  SIN(VP) 

C23PP  =  COS(APP) 

S23PP  =  SINfAPP 
C23PN  =  COS  APN) 

S23PN  =  SIN (APN) 

C4P1  =  C0S(V4P1 
S4P1  =  SIN(V4P1) 

C4P2  -  C0S(V4P2) 

S4P2  -  SIN(V4P2j 

COMPUTE  FIRST  TERMS  OF  THE  ARCTAN2  FUNCTIONS 
T5A1PPPP  =  -  (R13  ♦  ((CIP  ♦  C23PP  *  C4P1)  + 

T5A2PPPP  =  -  (R23  *  ((SlP  *  C23PP  *  C4Pl)  - 

T5A3PPPP  =  R33  *  S23PP  *  C4P1 
T5APPPP  =  T5A1PPPP  +  T5A2PPPP  +  T5A3PPPP 
T5A1PPNP  =  (R13  *  ((CIP  ♦  C23PN  *  C4P2)  + 

T5A2PPNP  =  -  (R23  *  ((SIP  *  C23PN  *  C4P2)  - 

T5A3PPNP  =  R33  *  S23PN  *  C4P2 
T5APPNP  =  T5A1PPNP  +  T5A2PPNP  +  T5A3PPNP 

COMPUTE  SECOND  TERMS  O'"  THE  ARCTAN2  FUNCTIONS 
T5B1PPPP  =  -  (CIP  *  S23PP  *  R13) 

T5B2PPPP  /sip  *  S23PP  *  R23) 

T5B3PPPP  =  (C23PP  *  R33) 

T5BPPPP  T5B1PPPP  +  T5B2PPPP  +  T5B3PPPP 
T5B1PPNP  ^  (CIP  *  S23PN  *  R13) 


SIP  *  S4P1))) 
CIP  *  S4P1))) 


SIP  *  S4P2))) 
CIP  *  S4P2))) 
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T5B2PPNP  =  -  (SIP  *  S23PN  ♦  R23) 

T5B3PPNP  =  -  (C23PN  ♦  R33) 

T5BPPNP  =  T5B1PPNP  +  T5B2PPNP  +  T5B3PPNP 

C  COMPUTE  VRIST  PITCH  ANGLES  USING  1RCTAN2  FUNCTION 
A5P1  =  ATAN2(T5APPPP,T5BPPPP) 

A5P2  =  ATAN2(T5APPNP,T5BPPNP) 

RETURN 

END 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 

C  DETERMINATION  OF  HAND  ROLL  ANGLES 

SUBROUTINE  HROLL ( A , A6P1 , A6P2 , VP , APP , APN , A4P1 , A4P2 , A5P1 , A5P2) 
INTEGER  I,J 

REAL  A (4 , 4) , A6P1 , A6P2 , VP , APP , APN , A4P1 , A4P2 , A5P1 , A5P2 , 

SPPPPPAl , PPPPPA2 , PPPPPA3 ,PPPPPA , PPNPPAl , PPNPPA2 , PPNPPA3 , PPNPPA 
SPPPPPB 1 , PPPPPB2 , PPPPPB3 , PPPPPB , PPNPPB 1 , PPNPPB2 , PPNPPB3 , PPNPPB 

C  INITIALIZE  LOCAL  VARIABLES  TO  0.0 
PPPPPAl  =0.0 
PPPPPA2  =0.0 
PPPPPA3  =0.0 
PPPPPA  =0.0 
PPNPPAl  =0.0 
PPNPPA2  =0.0 
PPNPPA3  =0.0 
PPNPPA  =0.0 
PPPPPBl  =0.0 
PPPPPB2  =0.0 
PPPPPB3  =0.0 
PPPPPB  =0.0 
PPNPPBl  =0.0 
PPNPPB2  =0.0 
PPNPPB3  =0.0 
PPNPPB  =0.0 

C  INITIALIZE  VRIST  ROLL  ANGLES  TO  0.0 
A6P1  =  0.0 
A6P2  =  0.0 

C  SETUP  ROTATION  PARAMETERS 
Rll  =  A(l,l) 

R21  =  A(2,l) 

R31  =  A(3,l) 

C  SETUP  UP  TRIG.  FUNCTIONS 
CIP  =  COS(VP) 

SIP  =  SIN(VP) 

C23PP  =  COS (APP) 

S23PP  =  SIN(APP) 

C23PN  =  COS (APN) 

S23PN  =  SIN (APN) 

C4P1  =  C0S(A4P1) 

S4P1  =  SIN(A4P1) 

C4P2  =  C0S(A4P2) 

S4P2  =  SIN(A4P2) 

C5P1  =  C0S(A5P1) 

S5P1  =  SIN(A5P1) 

C5P2  =  C0S(A5P2) 

S5P2  =  SIN(A5P2) 
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C  COlPIITE  THE  FIHST  TE&IS  FOR  THE  AHCTAN2  FUNCTION 

PPPPPAl  =  Hll  ♦  ((CIP  ♦  C23PP  *  S4P1)  -  (SIP  *  C4P1)) 

PPPPPA2  =  R21  ♦  ((SIP  ♦  C23PP  *  S4P1)  +  (CIP  ♦  C4P1)) 

PPPPPA3  =  R31  ♦  (S23PP  ♦  S4P1) 

PPPPPA  =  -  PPPPPAl  -  PPPPPA2  +  PPPPPA3 

PPNPPAl  =  Rll  ♦  ((CIP  ♦  C23PN  *  S4P2)  -  (SIP  ♦  C4P2)) 

PPNPPA2  =  R21  ♦  ((SIP  ♦  C23PN  ♦  S4P2)  +  (CIP  ♦  C4P2)) 

PPNPPA3  =  R31  ♦  (S23PN  *  S4P2) 

PPNPPA  =  -  PPNPPAl  -  PPNPPA2  +  PPNPPA3 
C  COMPUTE  THE  SECOND  TERMS  FOR  THE  ARCTAN2  FUNCTION 

PPPPPBl  =  Rll  *  (C5P1  ♦  ((CIP  *  C23PP  ♦  C4P1)  +  (SIP  *  S4P1)) 

$  -  (CIP  *  S23PP  *  S5P1)) 

PPPPPB2  =  R21  *  (C5P1  *  ((SIP  ♦  C23PP  *  C4P1)  -  (CIP  *  S4P1)) 

$  -  (SIP  ♦  S23PP  *  S5P1)) 

PPPPPB3  =  R31  *  ((S23PP  ♦  C4P1  ♦  C5P1)  +  (C23PP  ♦  S5P1)) 

PPPPPB  =  PPPPPBl  +  PPPPPB2  -  PPPPPB3 

PPNPPBl  =  Rll  *  (C5P2  ♦  ((CIP  ♦  C23PN  ♦  C4P2)  +  (SIP  ♦  S4P2)) 

$  -  (CIP  ♦  S23PN  *  S5P2)) 

PPNPPB2  =  R21  *  (C5P2  *  ((SIP  *  C23PN  *  C4P2)  -  (CIP  *  S4P2)) 

$  -  (SIP  *  S23PN  *  S5P2)) 

PPNPPB3  =  R31  *  ((S23PN  *  C4P2  *  C5P2)  +  (C23PN  *  S5P2)) 

PPNPPB  =  PPNPPBl  +  PPNPPB2  -  PPNPPB3 
C  COMPUTE  THE  HAND  ROLL  ANGLE  USING  THE  ARCTAN2  FUNCTION 
A6P1  =  ATAN2 (PPPPPA, PPPPPB) 

A6P2  =  ATAN2(PPNPPA, PPNPPB) 

RETURN 

END 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
C  RADIAN  TO  DEGREE  CONVERSION  ROUTINE 

SUBROUTINE  RADEG (RAD 1 , RAD2 , DEGl , DEG2) 

REAL  RADI, RAD2, DEGl, DEG2, PI 
C  INITIALIZE  LOCAL  VARIABLES  AND  RETURNED  VALUES 
DEGl  =  0.0 

DEG2  =  0.0 

C  DECLARE  CONSTANTS 

PI  =  3.141592653589792 

C  PERFORM  CONVERSION 

DEGl  =  RADI  *  180.0  /  PI 

DEG2  =  RAD2  *  180.0  /  PI 

RETURN 
END 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
C  CHECK  FOR  VALIDITY  OF  SOLUTIONS 
SUBROUTINE  VALID (A) 

REAL  A(4,7) 

INTEGER  I,J 

C  CHECK  FOR  VALIDITY  ON  ALL  JOINTS. IF  OUT  OF  RANGE, FIRST  COLUMN=1.0 

C  NOTE  THAT  THE  RANGES  ARE  OFFSET  BY  0.01  DEGREES  TO  TAKE  CARE 
C  OF  COMPUTATIONAL  ERRORS  CAUSED  BY  THE  MACHINE. 

DO  200  I  =  1,4 

C  VAIST  RANGE  IS  FROM  +  147  TO  -147  DEGREES. 

IF(ABS(A(I,2)  .GT.  147.01)  .OR. 

C  SHOULDER  RANGE  IS  FROM  +56  TO  -236  DEGREES. 

$  ((A(I,3)  .GT.  56.01)  .OR.  (A(I,3)  .LT.  -236.01))  .OR. 


129 


C  ELBOV  RANGE  IS  FROM  +  56  TO  -236  DEGREES. 

$  ((A(I,4)  .GT.  56.01)  .OR.  (A(I,4)  .LT.  -236.01))  .OR. 

C  VRIST  ROLL  IS  CONTINDODS.  RANGE  IS  *)-  360.0  DEGREES. 

$  ABS(A(I,5)  .GT.  360.01)  .OR. 

C  VRIST  PITCH  RANGE  IS  FROM  +  90  TO  -  90  DEGREES. 

$  ABS(A(I,6)  .GT.  90.01)  .OR. 

C  HAND  ROLL  IS  CONTINUOUS.  RANGE  IS  *1-  360  DEGREES 
$  ABS(A(I,7)  .GT.  360.01))  THEN 

C  IF  OUT  OF  RANGE, SET  FLAG  (COLUMN  1  OF  RESPECTIVE  ROV)  =1.0 
A(I,1)  =  1.0 
ENDIF 
200  CONTINUE 
RETURN 
END 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 

C  NORMALIZE  THE  COMPUTED  RESULTS  SO  THAT  EACH  ANGLE  RANGES 

C  FROM  -180.0  TO  180.0  DEGREES 
SUBROUTINE  NORMAL (A) 

REAL  A(4,7) 

INTEGER  I,J 

C  NORMALIZE  THE  ANGLES  TO  BETWEEN  -180  AND  +180  DEGREES 
DO  701  I  =  1,4 
DO  701  J  =  2,7 
IF(A(I,J)  .GT.  180.0)  THEN 
A(I,J)  =  A(I.J)  -  360.0 
ELSEIF(A(I,J)  .LT.  -180.0)  THEN 
A(I,J)  =  A(I,J)  +  360.0 
ENDIF 

701  CONTINUE 
RETURN 
END 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 


130 


ca  ra  rt  «  o 


APPENDIX  5 


lEELIN  6500  lANIPULATOR  VORKSPiCE  DEVELOPMENT  -  FORTRAN  CODE 
VERTICAL  VORKSPACE  DEVELOPMENT 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

PROGRAM  VSPACE.FOR  :==  MERLIN  6500  VERTICAL  VORKSPACE  DRAVIN6 
BY  ;==  RANVIR  S.  SOLANII 

VRIGHT  STATE  UNIVERSITY 
DAYTON,  OH  45435. 

GRAPHICHS  PACKAGE  USED  :  DISSPLA 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

REAL  I(1,2),Y(1,2),S,E,P,A2,A4,D6,PI 
C  SET  VALUE  OF  h. 

PI  =  3.141592653589792 

C  SETUP  KINEMATIC  PARAMETERS  FOR  THE  MERLIN  6500  ROBOT. 

C  A2  IS  THE  LENGTH  OF  THE  UPPER  ARM. 

A2  =  17.38 

C  A4  IS  THE  LENGTH  OF  THE  LOVER  ARM. 

A4  =  17.24 

C  D6  IS  THE  DISTANCE  FROM  THE  VRIST  PIN  TO  THE  TIP  OF  THE  ARM. 

D6  =  3.5 

C  TVO  TYPES  OF  OUTPUT  ARE  ALLOVBD  BY  THIS  PROGRAM,  HARDCOPY  BY  USING  C 
THE  OUTPUT  FILE  STD00001.DAT,  AND  SCREEN  OUTPUT  ON  A  TEKTRONIX 
C  4010  SCREEN,  VHICH  CAN  BE  SCREEN- DUMPED. 

C  SETUP  FOR  OUTPUT  FILE  STD00001.DAT.  OUTPUT  FILE  IS  CURRENTLY 
C  ENABLED. 

CALL  TALARS 

C  SETUP  FOR  TEKTRONIX  4010  SCREEN  OF  HIGH  RESOLUTION. 

C  THIS  OPTION  IS  CURRENTLY  DISABLED. 

C  CALL  TEKALL (4010, 960, 0,1,0) 

C  SETUP  PAGE  SIZE  OF  8"  x  8” 

CALL  PAGE(8. 0,8.0) 

C  SETUP  PLOT  AREA  OF  7.5"  x  7.5".  A  BORDER  OF  1/2"  IS  NECESSARY. 

CALL  AREA2D(7.5,7.5) 

C  FRAME  PLOT  AREA. 

CALL  FRAME 

C  SETUP  GRAPH  SCALE  (X  RANGE  FROM  -50  TO  50,  Y  SAME  AS  X) 

CALL  GRAF (-50.0,’ SCALE ’,50.0,-50.0,’ SCALE ’,50.0) 

C  VRITE  TITLE  ON  PLOT 

^  CALL  RLMESS ( ’ MERLIN  6500  VERTICAL  VORKSPACE ’ , 30 , -  20 . , -  49 . ) 

C  DRAVING  OF  THE  LINKS  AS  EACH  LINK  MOVES  DEVELOPS  THE  VORKSPACE 
C  OF  THE  ROBOT  ARM.  THERE  ARE  ONLY  THREE  VERTICAL  PLANE  MOTIONS, 

C  SHOULDER  MOTION,  ELBOV  MOTION,  AND  VRIST  PITCH  MOTION. 

C  DRAVING  IN  DISSPLA  IS  DONE  BY  CONNECTING  TVO  POINTS. 

C  EACH  POINT  IS  SPECIFIED  BY  ITS  ’X’  AND  ’Y’  POSITIONS. 

C  ’X’  IS  POSITIVE  TO  RIGHT  OF  SCREEN,  ’Y’  IS  POSITIVE  UPVARDS. 

C  NO  TRANSFORMATIONS  ARE  NEEDED,  AS  THE  VERTICAL  DIRECT  KINEMATICS 
C  HAVE  BEEN  COMPUTED  VITH  THE  BASE  FRAME  SET  UP  ACCORDING  TO  THE 
C  GRAPHICS  FRAME,  i.e.,  VITH  X  POSITIVE  TO  THE  RIGHT,  Y  POSITIVE 
C  UPVARDS.  THE  X  AND  Y  POSITIONS  OF  EACH  POINT  AT  THE  END  OF  THE 
C  LINK  ARE  COMPUTED  AND  STORED.  THUS,  [X(l,l),  Y(l,l)]  ARE  THE  X,Y 


131 


C  COORDINATES  OF  THE  FIRST  POINT,  VHILE  [1(1,2),  Y(l,2)]  ARE  THE 
C  COORDINATES  OF  THE  SECOND  POINT. 

C 

C  DRAV  GROUND  LEVEL  LINE 
C 

C  X  AXIS  POSITION  OF  GROUND  LEVEL  LINE 
X(l,l)  =  -45.0 
X(l,2)  =  45.0 

C  V  AXIS  POSITION  OF  GROUND  LEVEL  LINE 
V(l,l)  =  -46.45 
V(l,2)  =  -46.45 
C  DRAV  GROUND  LEVEL  LINE 
CALL  CURVE(X,V,2,0) 

C  INDICATE  CENTER  OF  GROUND  LEVEL  LINE 
X(l,l)  =  0.0 
X(l,2)  =  0.0 
V(l,l)  =  -45.0 
V(l,2)  =  -48.0 
CALL  CURVE (X,V, 2,0) 

C  INDICATE  GROUND  LEVEL  LINE  ON  PLOT. 

CALL  RLMESS(’BASE  LEVEL’ ,10,- 5.0,- 45.0) 

C  DEVELOP  VERTICAL  VORKSPACE  USING  VERTICAL  MOTION  JOINT  KINEMATICS. 

C 

C  INDICATE  STEP  SIZE  FOR  EACH  JOINT  ON  THE  PLOT. 

CALL  RLMESS( ’SHOULDER  STEP  SIZE  =  9.125  DEGREES’ ,34,-48. ,46.) 

CALL  RLMESSf’ELBOV  STEP  SIZE  =  9.125  DEGREES ’,3 1,-48. ,43.) 

CALL  RLMESS(’VRIST  PITCH  STEP  SIZE  =10.0  DEGREES’ ,36,-48. ,40.) 

C  DRAV  UPPER  ARM  LINK  FROM  0,0  TO  END  OF  LINK  ACCORDING  TO 
C  THE  CURRENT  SHOULDER  ANGLE,  IN  STEPS  INDICATED  ABOVE. 

DO  10  S  =  237.0,-57.0,-9.125 
X(l,l)  =  0.0 
V(l,l)  =  0.0 

X(l,2)  =  A2  ♦  COS(S  *  PI  /  180.0) 

V(l,2)  =  A2  ♦  SINfS  ♦  PI  /  180.0) 

CALL  CURVE (X,V, 2,0) 

C  DRAV  LOVER  ARM  LINK  FROM  BEGINNING  TO  END  OF  LINK  ACCORDING  TO 
C  THE  CURRENT  ELBOV  ANGLE,  IN  STEPS  INDICATED  ABOVE. 

DO  10  E  =  146.0,-146.0,-9.125 
X(l,l)  =  A2  *  COS(S  *  PI  /  180.0) 

V(l,l)  =  A2  ♦  SIN(S  ♦  PI  /  180.0) 

X(l,2)  =  A2  ♦  COS(S  *  PI  /  180.0)  + 

$  A4  ^  COS((S  +  E)  *  PI  /  180.0) 

V(l,2)  =  A2  *  SIN(S  ♦  PI  /  180.0)  + 

$  A4  *  SIN((S  +  E)  *  PI  /  180.0) 

CALL  CURVE(X,V,2,0) 

C  DRAV  VRIST  PITCH  LINK  (VRIST  PIN  TO  TIP  OF  VRIST)  ACCORDING  TO  THE  C 
CURRENT  VRIST  PITCH  ANGLE,  IN  STEPS  INDICATED  ABOVE. 

DO  10  P  =  90.0,-90.0,-10.0 
X(l,l)  =  A2  *  COS(S  ♦  PI  /  180.)  + 

$  A4  ^  COS((S  +  E)  ♦  PI  /  180.0) 

V(l,l)  =  A2  *  SIN(S  ♦  PI  /  180.)  + 

%  A4  *  SIN((S  +  E)  ♦  PI  /  180.0) 

X(l,2)  =  X(l,l)  +  D6  ♦  COSr(S  +  E  +  P)  ♦  PI  /  180.0) 

V(l,2)  =  V(l,l)  +  D6  ♦  SIN((S  +  E  +  P)  *  PI  /  180.0) 
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CALL  CURVE(X,Y,2,0) 

10  CONTINUE 

C  CLOSE  ALL  DEVICES  AND  EXIT  DISSPLA. 

CALL  ENDPL(O) 

CALL  DONEPL 
CALL  EXIT 
C  EXIT  PROGRAM 
STOP 
END 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 


HORIZONTAL  WORKSPACE  DEVELOPMENT 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
PROGRAM  HSPACE.FOR  :==  MERLIN  6500  HORIZONTAL  WORKSPACE  DRAWING 
BY  :==  RANVIR  S.  SOLANKI 

WRIGHT  STATE  UNIVERSITY 
DAYTON,  OH  45435. 

GRAPHICHS  PACKAGE  USED  :  DISSPLA 
CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
REAL  X(1,2),Y(1,2),W,S,P,L,A2,A4,D,D6,PI 
C  SET  CONSTANT  mUES  (  PI  ) 

PI  =  3.141592653589792 

C  SETUP  KINEMATIC  PARAMETERS  FOR  THE  MERLIN  6500  ROBOT. 

C  A2  IS  THE  LENGTH  OF  THE  UPPER  ARM. 

A2  =  17.38 

C  A4  IS  THE  LENGTH  OF  THE  LOWER  ARM. 

A4  =  17.24 

C  L  IS  THE  COMBINED  LENGTH  OF  THE  UPPER  AND  LOWER  ARMS. 

L  =  A2  +  A4 

C  D  IS  THE  OFFSET  OF  THE  LOWER  ARM  FROM  THE  YO  AXIS. 

D  =  12.0 

C  D6  IS  THE  DISTANCE  FROM  THE  WRIST  PIN  TO  THE  TIP  OF  THE  ARM. 

D6  =  3.5 

C  TWO  TYPES  OF  OUTPUT  ARE  ALLOWED  BY  THIS  PROGRAM,  HARDCOPY  BY 
C  USING  THE  OUTPUT  FILE  STD00001.DAT,  AND  SCREEN  OUTPUT  ON  A 
C  TEKTRONIX  4010  SCREEN,  WHICH  CAN  BE  SCREEN- DUMPED . 

C  SETUP  FOR  OUTPUT  FILE  STD00001.DAT.  OUTPUT  FILE  IS  CURRENTLY 
C  ENABLED. 

CALL  TALARS 

C  SETUP  FOR  TEKTRONIX  4010  SCREEN  OF  HIGH  RESOLUTION. 

C  THIS  OPTION  IS  CURRENTLY  DISABLED. 

C  CALL  TEKALL(4010, 960, 0,1,0) 

C  SETUP  PAGE  SIZE  OF  8"  x  8” 

CALL  PAGE(8. 0,8.0) 

C  SETUP  PLOT  AREA  OF  7.5"  x  7.5".  A  BORDER  OF  1/2"  IS  NECESSARY. 

CALL  AREA2D(7.5,7.5) 

C  FRAME  PLOT  AREA. 

CALL  FRAME 

C  SETUP  GRAPH  SCALE  (X  RANGE  FROM  -50  TO  50,  Y  SAME  AS  X) 

CALL  GRAF (-50.0,’ SCALE ’,50.0,-50.0,’ SCALE ’,50.0) 
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C  WRITE  TITLE  ON  PLOT 

CALL  RLMESS( ’MERLIN  6500  HORIZONTAL  WORKSPACE’ ,32,-25. ,-49.) 

C 

C  DRAWING  OF  THE  LINKS  AS  EACH  LINK  MOVES  DEVELOPS  THE  WORKSPACE 
C  OF  THE  ROBOT  ARM.  THERE  ARE  ONLY  TWO  HORIZONTAL  PLANE  MOTIONS, 

C  THE  WAIST  MOTION  AND  THE  WRIST  YAW  (  ACTUALLY  THE  WRIST  PITCH 
C  MOTION  AFTER  A  SET  ROLL  OF  90  DEGREES  ). 

C 

C  DRAWING  IN  DISSPLA  IS  DONE  BY  CONNECTING  TWO  POINTS. 

C  EACH  POINT  IS  SPECIFIED  BY  ITS  ’I’  AND  ’Y’  POSITIONS. 

C  ’X’  IS  POSITIVE  TO  RIGHT  OF  SCREEN,  ’Y’  IS  POSITIVE  UPWARDS. 

C  NO  TRANSFORMATIONS  ARE  NEEDED,  AS  THE  HORIZONTAL  DIRECT  KINEMATICS 
C  HAVE  BEEN  COMPUTED  WITH  THE  BASE  FRAME  SET  UP  ACCORDING  TO  THE 
C  GRAPHICS  FRAME,  i.e.,  WITH  I  POSITIVE  TO  THE  RIGHT,  Y  POSITIVE 
C  UPWARDS.  THE  X  AND  Y  POSITIONS  OF  EACH  POINT  AT  THE  END  OF  THE 
C  LINK  ARE  COMPUTED  AND  STORED.  THUS,  [X(l,l),  Y( 1,1)1  ARE  THE  X,Y 
C  COORDINATES  OF  THE  FIRST  POINT,  WHILE  [X(l,2),  Y(l,2)]  ARE  THE 
C  COORDINATES  OF  THE  SECOND  POINT. 

C 

C  DRAW  REFERENCE  LINE  THROUGH  0,0 
C 

C  X  AXIS  POSITION  OF  REFERENCE  LINE. 

X(l,l)  =  -45.0 
X(l,2)  =  45.0 

C  Y  AXIS  POSITION  OF  REFERENCE  LINE. 

Y(l,l)  =  0.0 
Y(l,2)  =  0.0 
C  DRAW  REFERENCE  LINE. 

CALL  CURVE (X,Y, 2,0) 

C  INDICATE  CENTk  OF  RE^RENCE  LINE. 

X(l,l)  =  0.0 
X(l,2)  =  0.0 
Y(l,l)  =  2.0 
Y(l,2)  =  -2.0 
CALL  CURVE(X,Y,2,0) 

C  INDICATE  REFERENCE  LINE  ON  PLOT. 

CALL  RLMESS(’REF.’, 4,-48. 0,-3. 0) 

CALL  RLMESS(’LINE’,4,43.0,-3.0) 

C  DEVELOP  HORIZONTAL  WORKSPACE  USING  HORIZONTAL  JOINT  KINEMATICS. 

C 

C  INDICATE  STEP  SIZE  FOR  EACH  JOINT  ON  THE  PLOT. 

CALL  RLMESSf ’WAIST  STEP  SIZE  =3.0  DEGREES’ ,29,-48. ,46.) 

CALL  RLMESSf’WRIST  YAW  STEP  SIZE  =  10.0  DEGREES’ ,34,-48. ,43.) 

C  DRAW  SHOULDER  OFFSET  LINK  FROM  0,0  TO  END  OF  LINK  ACCORDING  TO 
C  THE  CURRENT  WAIST  ANGLE  ’W’,  IN  STEPS  INDICATED  ABOVE. 

DO  10  W  =  147.0,-147.0,-3.0 
X(l,l)  =  0.0 
Y(l,l)  =  0.0 

X(l,2)  =  -D  *  COS(W  *  PI  /  180.0) 

Y(l,2)  =  -D  *  SIN(W  *  PI  /  180.0) 

CALL  CURVE(X,Y,2,0) 

C  DRAW  ARM  LINK  FROM  SHOULDER  JOINT  TO  WRIST  PIN  WITH  THE  SHOULDER 
C  ANGLE  SET  TO  90  DEGREES. 

X(l,l)  =  -D  *  COS(W  *  PI  /  180.0) 
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1(1,2)  =  -D  *  COS(V  *  PI  /  180.0)  + 

$  L  *  C0S((V  +  90.0)  *  PI  /  180.0) 

Y(l,l)  =  -D  *  SIN(V  *  PI  /  180.0) 

Y(l,2)  =  -D  *  SIN(V  *  PI  /  180.0)  + 

$  L  *  SIN((V  +  90.0)  *  PI  /  180.0) 

CALL  CDaVE(X,Y,2,0) 

C  DRAV  VaiST  YAVik  LINK  (VRIST  PIN  TO  TIP  OF  VRIST)  ACCORDING  TO 
C  THE  CURRENT  VRIST  YAV  ANGLE,  IN  STEPS  INDICATED  ABOVE. 

DO  10  P  -  0.0,-180.0,-10.0 
1(1,1)  =  -D  *  COS(V  *  PI  /  180.)  + 

S  L  *  COS((V  +  90.0)  *  PI  /  180.0) 

Y(l,l)  =  -D  *  SIN(V  *  PI  /  180.)  + 

$  L  *  SIN((V  +  90.0)  *  PI  /  180.0) 

X(l,2)  =  X(l,l)  -  D6  *  SIN((V  +  90.0  +  P)  ♦  PI  /  180.0) 

Y(l,2)  =  Y(l,l)  +  D6  *  COS((V  +  90.0  +  P)  *  PI  /  180.0) 

CALL  CDRVE(X,Y,2,0) 

10  CONTINUE 

C  CLOSE  ALL  DEVICES  AND  EXIT  DISSPLA. 

CALL  ENDPL(O) 

CALL  DONEPL 
CALL  EXIT 
C  EXIT  PROGRAM 
STOP 
END 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
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APPENDIX  7 


DIRECT  KINEMATICS  SIMULATION  FOR  THE  UTAH/MIT  DEXTEROUS  HAND 

PROGRAM  UDKIN.FOR 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

UTAH/MIT  DEXTROUS  HAND  DIRECT  KINEMATICS  SIMULATION  PROGRAM 
BY  :==  RANVIR  S.  SOLANKI 

VRIGHT  STATE  UNIVERSITY 
DAYTON,  OHIO  -  45435 
CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
C  MAIN  PROGRAM 

C  DEFINE  REAL  ft  INTEGER  VARIABLES. 

REAL  ANG(4,4) ,T(4,4) ,F1(4,4) ,F2(4,4) ,F3(4,4) ,PT,PF1 ,PF2,PF3 
INTEGER  I, J, RERUN, FLAG 

C  AS  MULTIPLE  DIGITS  ARE  INVOLVED  IN  THIS  SIMULATION,  VE  REPRESENT 
C  THE  THUMB  BY  USING  VARIABLES  ENDING  IN  ’T’,  FINGER  1  VARIABLES 
C  WITH  ’FI’,  FINGER  2  VITH  ’F2’  AND  FINGER  3  WITH  ’F3’. 

C  DESCRIBE  PROGRAM. 

PRINT  *,’  THIS  PROGRAM  PERFORMS  A  MATHEMATICAL  SIMULATION  ’ 
PRINT  *,’  OF  THE  KINEMATICS  OF  THE  UTAH/MIT  DEXTROUS  HAND.  ’ 

10  PRINT  *,’  ’ 

C  OPEN  OUTPUT  FILE  ’HANDKIN.OUT’ . 

0PEN(UNIT=6 , STATUS= ’ NEV ’ , FILE= ’ HANDDKIN . OUT ’ ) 

C  FIND  THE  USER- DEFINED  ANGLES  FOR  THE  THUMB  AND  FINGERS. 

CALL  ANGLES (ANG) 

C  THE  USER  HAS  THE  OPTION  OF  FINDING  THE  POSITION  V.R.T.  ANY 
C  POINT  ON  THE  FINGERS  OR  THUMB,  OR  FINDING  THE  POSITION  OF 
C  THE  LAST  JOINT.  DETERMINE  IF  USER  WANTS  POSITION  V.R.T. 

C  LAST  JOINT  ON  FINGERS  OR  IF  THE  POSITION  REQUIRED  IS  V.R.T. 

C  A  PARTICULAR  POINT  ON  THE  FINGER  OR  THUMB. 

C  SET  DEFAULT  TO  BE  POSITION  V.R.T.  LAST  JOINT. 

FLAG  =  0 

CALL  P0SIT(FLAG,PT,PF1,PF2,PF3) 

C  COMPUTE  THE  DIRECT  KINEMATIC  TRANSFORM  MATRICES  [T] , [FI] , [F2] , [F3] 
CALL  DIRKIN(ANG,T,F1,F2,F3,FLAG,PT,PF1,PF2,PF3) 

C  OUTPUT  THE  DATA  TO  SCREEN  AND  A  DATA  FILE  (HANDKIN.OUT) 

CALL  T0UT(T,F1,F2,F3) 

RERUN  =  0 

PRINT  * RERUN  SIMULATION  ?  (  1  =  YES,  0  =  NO  )  ==>  ’ 

REAOrs,*)  RERUN 
IF(RMUN  .EQ.  1)  THEN 
GOTO  10 
ENDIF 

C  CLOSE  OUTPUT  FILE 
CL0SE(UNIT=6) 

STOP 

END 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
C  ENTRY  OF  JOINT  ANGLES  BY  THE  USER 
SUBROUTINE  ANGLES (ANG) 

REAL  ANG(4,4),PI,ANGLE(4,4) 
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INTEGER  I,J,F 

C  TVO  MATRICES  ARE  DEFINED  HERE,  [ANG]  AND  [ANGLE] . 

C  MATRIX  [ANGLE]  IS  THE  MATRIX  VHERE  THE  USER  ENTERED  VALDES  FOR  THE 
C  JOINT  ANGLES  ON  THE  THUMB  (  ROV  1  ENTRIES  )  AND  ON  FINGERS  1,2,3. 

C  (  ROV  2,3,4  ENTRIES  )  ARE  STORED  IN  DEGREES. 

C  THE  DEGREE  VALUES  OF  THE  ANGLES  FOR  THE  JOINTS  ON  THE  THUMB  AND 
C  FINGERS  ARE  THEN  CONVERTED  TO  RADIANS  AND  STORED  IN  MATRIX  [ANG] . 

C  JOINT  0  DATA  OF  EACH  FINGER/THDMB  IS  STORED  IN  COLUMN  1,  JOINT  1 
C  DATA  l«  COLUMN  2,  JOINT  2  DATA  IN  COLUMN  3  AND  JOINT  3  DATA  IN 
C  COLUMN  4. 

C  ROV  1  REPRESENTS  THE  THUMB  VALUES,  ROV  2  REPRESENTS  FINGER  1 
C  VALDES,  ROV  3  REPRESENTS  FINGER  2  VALDES,  ROV  4  REPRESENTS  FINGER 
C  3  VALUES  IN  BOTH  MATRICES  [ANG]  AND  [ANGLE] 

C  DEFINE  CONSTANT  PI 

PI  =  3.141592653589792 

C  INITIALIZE  MATRIX  [ANG]  AND  [ANGLES]  ENTRIES  TO  0.0 
DO  100  I  =  1,4 
DO  100  J  -  1,4 

C  USER  ENTERED  ANGLES  MATRIX  (  VALUES  IN  DEGREES  ) 

ANGLE(I,J)  =  0.0 

C  JOINT  ANGLES  MATRIX  (  VALUES  IN  RADIANS  ) 

ANG(I,J)  =  0.0 

100  CONTINUE 

C  ENTRY  BY  USER  OF  THE  THUMB  ANGLES 
C  JOINT  0  ON  THUMB 

101  PRINT  *  ’ENTER  THUMB  JOINT  0  ANGLE  (-45  TO  -135  DEGREES)  ==>  ’ 
READ(5,’^)  ANGLE(1,1) 

C  JOINT  1  ON  THUMB 

102  PRINT  *  ’ENTER  THUMB  JOINT  1  ANGLE  (-15  TO  +60  DEGREES)  ==>  ’ 
READ(5,^)  ANGLE(1,2) 

C  JOINT  2  ON  THUMB 

103  PRINT  *  ’ENTER  THUMB  JOINT  2  ANGLE  (+6.5  TO  +90  DEGREES)  ==>  ’ 
READ(5,’^)  ANGLE(1,3) 

C  JOINT  3  ON  THUMB 

104  PRINT  *  ’ENTER  THUMB  JOINT  3  ANGLE  (0  TO  90  DEGREES)  ==>  ’ 
READ(5,’^)  ANGLE(1,4) 

C  USER  ENTRY  OF  JOINT  ANGLES  FOR  FINGERS 
DO  no  F  =  1,3 

C  JOINT  0  OF  FINGERS  1,2  ft  3 

111  PRINT  *,’  ENTER  FINGER  ’,  F 

PRINT  +,’  JOINT  0  ANGLE  (  65  TO  115  DEGREES  )  =->  ’ 

READ(5,*)  ANGLE((F+1),1) 

C  JOINT  1  OF  FINGERS  1,2  ft  3 

112  PRINT  *,’  ENTER  FINGER’,  F 

PRINT  *,’  JOINT  1  ANGLE  (  120  TO  190  DEGREES  )  -=>’ 

READ(5,*)  ANGLE((F+1),2) 

C  JOINT  2  OF  FINGERS  1,2  ft  3 

113  PRINT  *,’  ENTER  FINGER’,  F 

PRINT  * JOINT  2  ANGLE  (  3.5  TO  90  DEGREES  )  ==>  ’ 

READ(5,’^)  ANGLE((F+1),3) 

C  JOINT  3  OF  FINGERS  1,2  ft  3 

114  PRINT  *,’  ENTER  FINGER’,  F 

PRINT  * JOINT  3  ANGLE  (  0  TO  90  DEGREES  )  ^=>  ’ 

READ(5,’^)  ANGLE((F+1),4) 
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no  CONTINUE 

C  CONVERT  ANGLES  FROM  DEGREES  TO  RADIANS,  STORE  RADIAN  VALDES  IN 

C  MATRIX  [ANG] 

DO  115  I  =  1,4 
DO  115  J  =  1,4 

ANG(I,J)  =  ANGLE(I,J)  *  PI  /  180.0 

115  CONTINUE 

C  WRITE  TO  FILE 

VRITE(6,*)  ’  ’ 

VRITE(6,*)  ’  ANGLES  DATA  IN  DEGREES.  ’ 

VRITE(6,*t  ’  RQVS  REPRESENT  THE  DIFFERENT  DIGITS.  ’ 
VRITE(6,*)  ’  COLUMNS  REPRESENT  THE  JOINT  NUMBERS.  ’ 

DO  116  I  =  1,4 

VRITE(6,*)  (ANGLE(I,J).J=1,4) 

116  CONTINUE 
RETURN 
END 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCGCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 

C  USER  ENTRY  OF  POINT  ON  FINGERS  AND  THUMB  V.R.T.  WHICH  POSITION 

C  IS  TO  BE  REPORTED. 

SUBROUTINE  P0SIT(FLAG,AT,A1,A2,A3) 

REAL  AT , A1 , A2 , A3 , TMAX . F 1  MAX , F2MAa , F3MAX 
INTEGER  FLAG 

C  INITIALIZE  LOCALLY  COMPUTED  VARIABLES  TO  0.0 
AT  =  0.0 


A1  =  0.0 
A2  =  0.0 
A3  =  0.0 
PRINT 
PRINT 
PRINT 
PRINT 
PRINT 
PRINT 


’  ENTER  0  IF  THE  POSITION  DATA  REPORTED  IS  REQUIRED  ’ 

’  W.R.T.  THE  LAST  JOINT  ON  THE  FINGERS  AND  THUMB.  ’ 

’  ENTER  1  IF  THE  POSITION  DATA  IS  TO  BE  REPORTED  ’ 

’  W.R.T.  A  POINT  ON  THE  THUMB  AND  FINGERS  OTHER  ’ 

’  THAN  THE  LAST  JOINT  :  ==>  ’ 


READ(5,300)  FLAG 
300  FORMAT(I) 

IFfFLAG  .EQ.  1)  THEN 

C  DEFINE  THE  MAXIMUM  LENGTH  OF  LAST  LINK  OF  ALL  FINGERS. 
TMAX  =  1.125 
FIMAX  =  1.0625 
F2MAX  =  1.0625 
F3MAX  =  1.0625 

C  DETERMINE  POSITION  W.R.T.  WHICH  DATA  IS  TO  BE  REPORTED. 


301 


302 


PRINT 
PRINT 
READ(5,*) 
IF((AT 
PRINT  * 


( 


)iTION  TO 


IN  INCHES  )  W.R.T. 
BE  REPORTED 


THEN 


ENTER  DISTANCE  ALONG  THUMB 
WHICH  YOU  WANT  THE  POSIT] 

AT 

GT.  TMAX)  .OR.  (AT  .LT.  0.0)) 

’  >»  ENTRY  ERROR  «<  ’ 

PRINT  MAX.  LENGTH  OF  THUMB  LAST  L:NK 
GOTO  301 
ENDIF 

PRINT  ENTER  DISTANCE  ALONG  FINGER  1  (IN  INCHES)  W.R.T. 

PRINT  * WHICH  YOU  WANT  THE  POSITION  TO  BE  REPORTED  ==>  ’ 
READ(5,’^)  A1 


’, TMAX, ’INCHES’ 
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IF((A1  .GT.  FIMAX)  .OR.  (Al  .LT.  0.0))  THEN 
PRINT  »>  ENTRY  ERROR  «<  ’ 

PRINT  MAX.  LENGTH  OF  FINGER  1  LAST  LINK  =’ ,F1MAX, ’INCHES’ 
GOTO  302 
ENDIF 

303  PRINT  ENTER  DISTANCE  ALONG  FINGER  2  (IN  INCHES)  V.R.T.  ’ 

PRINT  *  ’  WHICH  YOU  WANT  THE  POSi flON  TO  BE  REPORTED  ==>  ’ 

READ (5,^)  A2 

IF((A2  .GT.  F2MAX)  .OR.  (A2  .LT.  0.0))  THEN 
PRINT  >»  EhRY  ERROR  «<  ’ 

PRINT  MAX.  LENGTH  OF  FINGER  2  LAST  LINK  =’ ,F2MAX, ’INCHES’ 
GOTO  303 
ENDIF 

304  PRINT  ENTER  DISTANCE  ALONG  FINGER  3  (IN  INCHES)  V.R.T.  ’ 

PRINT  * VHICH  YOU  VANT  THE  POSITION  TO  BE  REPORTED  ==>  ’ 
READ(5,-^)  A3 

IF((A3  .GT.  F3HAX)  .OR.  (A3  .LT.  0.0))  THEN 
PRINT  >>>  Em  ERROR  «<  ’ 

PRINT  MAX.  LENGTH  OF  FINGER  3  LAST  LINK  =’ ,F3MAX, ’INCHES’ 
GOTO  304 
ENDIF 
ENDIF 
RETURN 
END 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

C  DIRECT  KINEMATICS  IMPLEMENTATION  FOR  THE  UTAH/MIT  HAND 
SUBROUTINE  DIRKIN(ANG,T,F1,F2,F3,FLAG,AT,A1 ,A2,A3) 

INTEGER  I, J, FLAG 

REAL  ANG(4,4),T(4,4),F1(4,4),F2(4,4),F3(4,4),C1T,C2T,C23T, 
$C234T,SlT,S2T,S23T,S234t,A0T,AlT,A2T,A3T,DlT,PI,AT,Al,A2,A3, 
$A0F1,A0F2,A0F3,ANGF1(1,4),ANGF2(1,4),ANGF3(1,4),MAT(4,4) 

C  MATRIX  T  REFERS  TO  THE  POSITION  AND  ORIENTATION  MATRIX  OF  THUMB. 

C  MATRIX  FI  REFERS  TO  POSITION  AND  ORIENTATION  MATRIX  OF  FINGER  1. 

C  MATRIX  F2  REFERS  TO  POSITION  AND  ORIENTATION  MATRIX  OF  FINGER  2. 

C  MATRIX  F3  REFERS  TO  POSITION  AND  ORIENTATION  MATRIX  OF  FINGER  3. 

C  INITIALIZE  ALL  TRE  LOCALLY  COMPUTED  MATRICES 
DO  401  I  1.4 
DO  401  J  -  1,4 
T(I,J)  -  0.0 
F1(I,J)  0.0 

F2(I,J)  =  0.0 
F3(I,J)  .  0.0 

401  CONTINUE 

C  ASSIGN  THE  (4,4)  TERM  OF  ALL  KINEMATIC  MATRICES  TO  1.0 
T(4,4)  .  1.0 
Fl(4,4)  -  1.0 
F2(4,4)  1.0 

F3(4,4)  -  1.0 

C  THUMB  COMPUTATIONS 

C 

C  DEFINE  COSINES  A.NT  SINES  NEEDED  FOR  KINEMATICS  FOR  THE  THUMB 
CIT  ^  COS(ANC(]  ,1'i} 

C2T  COS  ANG(1.2)) 

C23T  -  C0S{ANG(1,2)  +  ANG(1,3)) 
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S2T  =  SIN(ANG(1,2)) 

S23T  =  SIN(ANG(1,2)  +  ANG(1,3)) 

S234T  =  SIN(ANG(1,2)  +  ANG(1,3)  +  ANG(1,4)) 

C  DEFINE  KINEMATIC  PARAMETERS  (A0,A1,A2,A3  k  Dl)  FOR  THUMB  IN 
C  INCHES. 

AOT  =  -  0.75 
AIT  =  0.375 
A2T  =  1.70 
A3T  =  1.30 


DIT  =  3.125 

COMPUTE  THE  KINEMATICS  POSITION  AND  ORIENTATION  MATRIX  FOR  THUMB, 


Tl 

Ti 

Tl 

T 

Tl 

Tl 

Tl 

Tl 

Tl 

Tl 

T 

Tl 


1,2 

1.3 

1.4 
2,1 
2,2 
2,3 
:2.4 

3.1 

3.2 

3.3 

3.4 


(A2T  ♦  C2T)  +  (A3T  *  C23T)) 


C2T)  +  (A3T  *  C23T)) 


CIT  *  C234T 
-  CIT  *  S234T 
SIT 

AOT  +  CIT  *  (AIT  + 

SIT  *  C234T 
-SIT  *  S234T 
-CIT 

SIT  *  (AIT  +  (A2T  ♦ 

S234T 
C234T 
0.0 

^  ^  (A2T  *  S2T)  +  (A3T  ♦  S23T)  +  DIT 

C  FING^lR  COMPUTATIONS 

C  THESE  TRANSFORM  MATRICES  ARE  COMPUTED  IN  SUBROUTINE  FINGER. 
C  FINGER  1  COMPUTATIONS 
C  STORE  ANGLE  DATA  IN  VECTOR  ANGF1(1,4) 

DO  402  I  =  1,4 
ANGF1(1,I)  =  ANG(2,I) 

402  CONTINUE 

C  SET  AO  TERM  FOR  FINGER  1 
AOFl  =  -1.375 
CALL  FINGER(ANGF1,A0F1,F1) 

C  FINGER  2  COMPUTATIONS 
C  STORE  ANGLE  DATA  IN  VECTOR  ANGF2(1,4) 

DO  404  I  =  1,4 
ANGF2(1,I)  =  ANG(3,I) 

404  CONTINUE 

C  SET  AO  TERM  FOR  FINGER  2 
A0F2  =  0.0 


CALL  FINGER(ANGF2,A0F2,F2) 

C  FINGER  3  COMPUTATIONS 
C  STORE  ANGLE  DATA  IN  VECTOR  ANGF3(1,4) 

DO  406  I  =  1,4 
ANGF3(1,I)  =  ANG(4,I) 

406  CONTINUE 

C  SET  AO  TERM  FOR  FINGER  3 
A0F3  =  1.1875 
CALL  FINGER(ANGF3,A0F3,F3) 

C  IF  THE  USER  HAS  REQUESTED  FOR  POSITION  TO  BE  REPORTED  V.R.T.  A 
C  POINT  ALONG  THE  LAST  LINK,  THEN  UPDATE  POSITION  VECTOR  TO  BE 
C  (POSITION  VECTOR  +  (NORMAL  VECTOR  *  LENGTH)). 


IF (FLAG  .EQ.  1)  THEN 
T(l,4)  =  T(l,4)  +  (T(l,l)  *  AT) 

T(2,4)  =  T(2,4)  +  (T(2,l)  ♦  AT) 

T(3,4)  =  T(3,4)  +  (T  3,1)  ♦  AT) 

Fl(l,4)  =  n(l,4)  +  (Fl(l,l)  ♦  Al) 

Fl(2,4)  =  Fl(2,4)  +  (Fl(2,l)  ♦  Al) 

Fl(3,4)  =  Fl(3,4)  +  (Fl(3,l)  *  Al) 

F2(l,4)  =  F2(l,4)  +  (F2(l,l)  *  A2) 

F2(2,4)  =  F2(2,4)  +  (F2(2,l)  *  A2) 

F2(3,4)  =  F2(3,4)  +  (F2(3,l)  *  A2) 

F3(l,4)  =  F3(l,4)  +  (F3(l,l)  *  A3) 

F3(2,4)  =  F3(2,4)  +  (F3(2,l)  *  A3) 

F3(3,4)  =  F3(3,4)  +  (F3(3,l)  *  A3) 

ENDIF 

RETURN 

END 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
C  OUTPUT  TO  SCREEN  AND  DATA  FILE  "HANDKIN . OUT” 

SUBROUTINE  TOUT(T,Fl,F2,F3) 

REAL  T(4,4),F1(4,4),F2(4,4  ,F3(4,4) 

INTEGER  I,J 

C  DESCRIBE  OUTPUT  IN  DATA  FILE 

VRITE(6,*)  ’  TRANSFORMATION  MATRICES  V.R.T.  BASE  FRAME  ’ 
VRITE(6,*)  ’  ’ 

C  WRITE  OUT  THUMB  MATRIX  [T] 

PRINT  THUMB  MATRIX  ’ 

WRITE (6,*)  ’  THUMB  MATRIX  » 

WRITE (6,*)  ’  ’ 

DO  501  I  =  1,4 
C  WRITE  TO  SCREEN 


WRITE(5,*)  (T(I,J),J=1,4) 

C  WRITE  TO  OUTPUT  FILE  HANDKIN.OUT 


501 


WRITEfe,*)  (T(I,J),J=1,4) 
CONTINUE 


PRINT  ’ 

C  WRITE  OUT  FINGER  1  MATRIX  [FI] 
PRINT  FINGER  1  MATRIX  ’ 
WRITE(6,*)  ’  ’ 

WRITE(6,*)  ’  FINGER  1  MATRIX  ’ 
WRITE(6,*)  ’  ’ 

DO  502  I  =  1,4 
C  WRITE  TO  SCREEN 


WRITE(5,*)  (F1(I,J),J=1,4) 

C  WRITE  TO  OUTPUT  nLE  HANDKIN.DAT 


WRITE (6,*) 
502  CONTINUE 


(F1(I,J),J=1,4) 


PRINT  ’ 

C  WRITE  OUT  FINGER  2  MATRIX  [F2] 
PRINT  FINGER  2  MATRIX  ^ 

WRITE(6,*)  ’  ’ 

WRITE(6,*)  ’  FINGER  2  MATRIX  ’ 
WRITE(6,*)  ’  ’ 

DO  503  I  =  1,4 
C  WRITE  TO  SCREEN 
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VRITE(5,*)  (F2(I,J),J=1,4) 

:  VaiTE  TO  OUTPUT  niE  lihKIN.DAl 
VRITE(6,*)  (F2(I,J),J=1,4) 
503  CONTINUE 

PRINT  ’ 

:  VRITE  OUT  FINGER  3  lATRII  [F3] 
PRINT  FINGER  3  MATRIX  * 
VRITE(6,*)  ’  ’ 

VRITE(6,*)  ’  FINGER  3  MATRIX 


VRITE(6,* 
VRITE(6,* 
DO  504  I 


DO  504  I  =  1,4 
C  VRITE  TO  SCREEN 

VRITE(5,*)  (F3(I,J),J=1,4) 

C  VRITE  TO  OUTPUT  FILE  HANDKIN.DAT 
VRITE(6,*)  (F3(I,J),J=1,4) 

504  CONTINUE 
RETURN 
END 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

C  KINEMATIC  COMPUTATIONS  FOR  FINGERS. 

SUBROUTINE  FINGER(A,AOF,KIN) 

REAL  A(1,4),KIN(4,4),A0F 
INTEGER  I,J 

C  INITIALIZE  LOCALLY  COMPUTED  MATRIX  [KIN] 

DO  601  I  =  1,4 
DO  601  J  =  1,4 


KIN< 

CONTINUI 


:,J)  =  0,0 


SET  TRANSFORM  MATRIX  KIN [4,4]  TERM  TO  1.0 
KIN(4,4)  =  1.0 
DEFINE  PI 

PI  =  3.141592653589792 

DEFINE  THE  VALUE  OF  ANGLE  PHI  IN  DEGREES  FOR  THE  FINGERS. 
PHI  =  12.0 

CONVERT  ANGLE  PHI  TO  RADIANS. 

RPHI  =  PHI  *  PI  /  180,0 
DEFINE  COSINES  AND  SINES  FOR  ANGLE  PHI. 

CPHI  =  COS(RPHI) 

SPHI  =  SIN(RPHI) 

DEFINE  COSINES  AND  SINES  FOR  FINGERS. 

CIF  =  C0S(A(1,1)) 

C2F  =  COS  A(l,2)) 

C23F  =  C0S(A(1,2)  +  A(l,3)) 

C234F  =  C0S(A(1,2)  +  A(l,3)  +  A(l,4)) 

SIF  =  SIN(A(1,1)) 

S2F  =  SIN(A(1,2)) 

S23F  =  SIN(A(1,2)  +  A(l,3)) 

S234F  =  SIN(A(1,2)  +  A(l,3)  +  A(l,4)) 

DEFINE  KINEMATIC  PARAMETERS  (Al,A2,A3  k  Dl)  FOR  FINGERS. 
AIF  =  -  1.2  ♦  SIN(30.0  ♦  PI  /  180.0) 

A2F  =  1.7 
A3F  =13 

DIF  =  (^25  /  CPHI)  +  (1.2  ♦  COS(30.0  *  PI  /  180.0)) 


Sills: 


Aa,3)) 
A(l,3)  + 


A(l,4)) 


'A(l,2)  +  A(l,3)) 
l(A(l,2)  +  A(l,3  +  A(l,4]) 

PIC  PARAMETERS  (Al,A2,A3  k  Dl)  FOR  FINGERS. 
I  *  SIN(30.0  ♦  PI  /  180.0) 
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C  COlPUTE  THE  EINEIATIC  POSITION  AND  OEIENTATION  lATRIl  FOR  FINGERS. 
IIN(1,1)  =  GIF  ♦  C234F 
IIN(1,2)  =  -  GIF  *  S234F 
nN(l,3)  =  SIF 

KIN(1,4)  =  AOF  +  GIF  ♦  (AlF  +  (A2F  ♦  G2F)  +  (A3F  *  G23F)) 
IIN(2,1)  =  (SPHI  *  S234P)  +  (GPHI  ♦  SIF  *  G234F) 

nN(2,2)  =  (SPHI  ♦  G234F)  -  (GPHI  ♦  SIP  *  S234P) 

IIN(2,3)  =  -  GPHI  *  GIF 

nN(2,4  =  (DIF  *  SPHI)  +  GPHI  ♦  SIP  ♦  (AlF  +  (A2F  *  G2P) 

$+  (A3F  G23F))  +  SPHI  ♦  ((A2F  ♦  S2P)  +  (A3F  *  S23F)) 

nN(3,l)  =  (Gm  *  S234F)  -  (SPHI  ♦  SIF  *  G234F) 

IIN(3,2)  =  (GPHI  *  G234F)  +  (SPHI  *  SIF  ♦  S234F) 

KIN  1 3  3)  =  ISPHI  *  GIF 

IIN(3!4)  =  (DIF  ♦  GPHI)  -  SPHI  ♦  SIF  *  (AlF  +  (A2F  ♦  G2F) 

$+  (A3P  *  G23F))  +  GPHI  ♦  ((A2F  ♦  S2F)  +  (A3F  ♦  S23F)) 

RETURN 

END 

GCCGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGG 
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COMPUTER  GRAPHICAL  SIMULATION  -  FORTRAN  CODE 
AND  DOCUMENTED  DATA  FILES 

PROGRAM  SIMULATION. FOR 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

DIMENSION  10(8,3,10,14) ,G0(3, 2, 10, 14) ,TR(4,4,10,14) ,B(6) ,C(6) , 
$F0CDS(2) ,AXDAT(8,3) ,XD(8,2,1000) ,LINiNDM(20) ,CF(4) ,M1(6,4) , 
$M2(6,4) ,M3(6,4,100) ,IMiTS(6,2) 

REAL  XO,TR,GO,XD,D,PI,X,Y,Z,ztaT,B,M,ANG,FOCUS,MAG, 

$  ANGLE , TEMP , XH , YE , ZH , XP , YP , ZP , LIMITS , Ml , M2 , M3 
INTEGER  NUM , ROBNUM , A , C , COUNT , BF , N 1 , N2 , N , ACTIVE , 

$  AF, OPTION, SET, EXECUTE, SYSNUM,LINKNUM, RECEIVE, HANDNUM(2) , 

$  R00MNUM,MERLNUM(2)  ,CF,  END, MONITOR,  INIT,nNNUM,  STEPS 
CHARACTER  DFILE*20 
LOGICAL  DRAV(6), ERASE,  REPEAT 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

C  SET-UP  PARAMETERS 

C  Z  IS  INITIAL  VIEWPOINT  IN  INCHES  ALONG  Z  AXIS 
Z=220 

C  ZPOINT  IS  TOTAL  INITIAL  DISTANCE  FROM  DRAWING  ORIGIN  TO  VIEWPOINT 
ZPOINT=220 

C  DRAW(1X5)  ARE  LOGICAL  VARIABLES 
C  IF  THE  LOGICAL  IS  FALSE  THAT  UNIT  WILL  NOT  BE  DRAWN 
C  IF  TRUE  -  THAT  UNIT  WILL  BE  DRAWN 
C  ROOM  =  DRAW(l) 

C  LEFTARM  MERLIN  =  DRAW(2) 

C  RIGHTARM  MERLIN  =  DRAW(3) 

C  LEFTHAND  UTAH  =  DRAW (4) 

C  RIGHTHAND  UTAH  =  DRAW(5) 

DRAW(l)  =  .TRUE. 

DRAW (2)  =  .TRUE. 

DRAW(3)  =  .FALSE. 

DRAW(4)  =  .TRUE. 

DRAW(5)  =  .FALSE. 

C  MAG  IS  INITIAL  MAGNIFICATION 
MAG=1 

C  REPEAT  IS  LOGICAL  TO  TELL  IF  MORE  THAN  ONE  VIEW  IS  TO  BE  DRAWN 
REPEAT  =  .FALSE. 

C  PI  VALUE  OF  PI 
PI=ACOS(-1.0) 

C  THE  ACTIVE  ROBOT  WILL  BE  THE  LEFTHANDED  ROBOT 
ACTIVE=1 

READ  ROOM.DAT 

XO  STORES  3- DIMENSIONAL  CORNER  POINTS  OF  ALL  LINKS 

EXAMPLE  XO(I,J,K,L) 

I  IS  THE  POINT  NUMBER 
J  IS  THE  3  DIMENSIONAL  POINT  VECTOR 
K  IS  THE  LINK  NUMBER 
L  IS  THE  SYSTEM  NUMBER 
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60  STORES  TRANSLATIONAL  AND  ROTATIONAL  VECTORS  ALL  LINKS 

60(I,J,K,L) 

I  IS  THE  TRANSLATION  VECTOR 
J  IS  THE  ROTATION  VECTER 
K  IS  THE  LINK  NUMBER 
L  IS  THE  SYSTEM  NUMBER 

LINKNUM  NUMBER  OF  LINKS  IN  EACH  SYSTEM(A  FINGER  IS  A  SYSTEM) 

ROBNUM  COUNTER  FOR  READING  ROBOTS 

ROBNUM  =  ROBNUM  +  1 

ROOMNUM  ROOM  SYSTEM  NUMBERING  VARIABLE 

ROOMNUM  =  ROBNUM 


FILE  ’R00M.DAT’  STORES  THE  3- DIMENSIONAL  POINTS  DEFINING  THE  ROOM 
0PEN(UNIT=7,FILE=’R00M.DAT’, STATUS  =  ’OLD’) 

READ  (7,*)  LINKNUM (ROOMNUM) 

READ  (7,*)  (((10(1, J,K, ROOMNUM), 1=1, 8) , J=l, 3) ,K= 

$  1, LINKNUM (ROBNUM)) 

READ  (7,*)  (((60(1, J,K, ROOMNUM), 1=1, 3), J=l, 2), 

$  K=l,LlkNUM(ROOMNUM)) 

CL0SE(UNIT=7) 

READ  MERL.DAT 


DO  87  N  =  1,2 
R0BNUM=R0BNUM+1 

MERLNUM  MERLIN  ROBOT  NUMBERING  VARIABLE 

MERLNUM(l)  LEFT  HAND  ,  MERLNUM(2)  RIGHT  HAND 
MERLkM(N)  =  ROBNUM 

RECEIVE  NUMBER  OF  SYSTEM  TO  BE  LINKED  TO  (ROBOT  TO  ROOM) 

RECEIVE (MERLNUM (N))  =  ROOMNUM 

MERL.DAT  STORES  THE  3- DIMENSIONAL  DATA  POINTS, TRANSLATION  VECTORS, 
AND  ROTATION  VECTORS 


0PEN(UNIT=7,FILE=’MERL. DAT’, STATUS  =  ’OLD’) 
READ  (7,*)  LINKNUM(ROBNDM) 

READ  (7,*)  (((XO(I,J,K,ROBNUM),I=1,8),J=1,3),1 
$  1,LINKNUM(R0BNUM)) 

READ  (7,*)  (((G0(I,J,K,R0BNUM),I=1,3),J=1,2), 
$  K=1,LINKNUM(R0BNUM)) 


CL0SE(UNIT=7) 
87  CONTikE 


ADJUST  TRANSLATION  MATRIX  FROM  FILE  ’MERL.DAT’  TO  BUILD  A 


RIGHT  HANDED  ROBOT 


G0(1,1,2,MERLNUM(2))=  X0(1, 1,2, ROOMNUM)- G0(1,1, 2, MERLNUM(2)) 
GO  (2 , 1 , 5  ,MERLNUM(2) )  =  -  GO  (2 , 1 ,5  ,MERLhM(2) 

GO ( 3 , 1 , 6 , MERLNUM ( 2) ) =  -60(3,1,6, MERLNUM ( 2 ) ) 

GO (3 , 1 , 7 ,MERLNUM (2) ) =  -  GO (3 , 1 , 7 , MERLNUM (2) ) 


CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
UTAH  HAND 


HANDNUM  SYSTEM  NUMBERING  VARIABLE 

HANDNUM(l)  LEFT  HAND  HANDNUM(2)  RIGHT  HAND 
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DO  111  N=l,2 

HANDNUM(N)=ROBNUM  +  1 
RECEIVE (HANDNUM(N))  =  MERLNDM(N) 

DO  98  1=1,4 

RECEIVE (HANDNDM(N)  +  I)  =  HANDNUM(N) 

98  CONTINUE 

C  ’DTAH.DAT’  IS  DATA  FILE  WHICH  STORES  THE  POINT  VECTORS, TRANSLATION  C 
VECTORS,  AND  ROTATION  VECTORS  WITH  RESPECT  TO  EACH  LINKS  OWN 
C  COORDINATE  SYSTEM. 

OPEN (DNIT=7 , FILE= ’ UTAH . DAT ’ , STATDS= ’ OLD ’ ) 

DO  109  L=l,5 

R0BNUM=R0BNDM+1 
READ (7,* 


$ 

109  CONTINUE 

CL0SE(UNIT=7) 

111  CONTINUE 

C  ADJUST  TRANSLATION  MATRIX  FROM  FILE  ’UTAH. DAT’  TO  BUILD  A 
C  RIGHT  HAND 

GO ( 1 , 1 , 2 , (HANDNUM(2) +1) ) =-60(1,1,2, (HANDNUM(2) +1 ) ) 

GO ( 1 , 1 , 2 , (HANDNUM (2) +2) ) =- GO ( 1 , 1 , 2 , (HANDNUM (2) +2) ) 
G0(l,l,2,(HANDNUM(2)+3))=-G0(l,l,2,(HANDNDM(2)+3)) 
G0(l,l,2,(HANDNUM(2)+4))=-G0(l,l,2,(HANDNUM(2)+4)) 

C 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

C  INITIAL  VIEWPOINT 
C 

C  INIT  INITIAL  SETUP  VARIABLE- CAUSES  PROGRAM  TO 
C  CALCULATE  INITIAL  VIEWING  ANGLES. 

C 

INIT  =  1 
GOTO  199 
119  INIT  =  0 
C 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

C  MONITOR  SETUP 
C 

C  MONITOR  TYPE  OF  WORKSTATION  VARIABLE 

C 

WRITE(6,*)’IF  WORKING  ON  A  TEKTRONICS  4010  -  ENTER  1’ 

WRITE(6,*)’IF  WORKING  ON  A  TEKTRONICS  4207  -  ENTER  2’ 

WRITE(6  *)’IF  WORKING  ON  A  REGIS  -  ENTER  3’ 

READ(6,*)  MONITOR 
WRITE (6,*) 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
C  INITIAL  DRAW 

C  SETTING  OPTION  EQUAL  TO  ZERO  TELLS  THE  PROGRAM  WHERE  TO  RETURN 
C  AFTER  THE  INITIAL  DRAWING. 

OPTION  =  0 

C  GO  TO  THE  DRAWING  ROUTINE 
GO  TO  587 


)LINKNDM(ROBNUM) 

D(7,*)(((XO(I,J,K,ROBNUM),I=1,8), 
J = 1 , 3) ,  K=1 ,  LINKNUM  (ROBhM) ) 
D(7,*)(((60(I,J,K,ROBNOM),I=1,3), 
J=1 ,2) ,K=1 ,LINKNOM(ROBNUM) ) 
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cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

C  MAIN  lENU 

c 

C  OPTION  MEND  SELECTION  VARIABLE 

C 

143  VRITE(6,*)’  OPTIONS’ 

VEITE(6,*)’  GO  TO  SET- DP  MEND  -  ENTER  1’ 

VRITE(6,*)’  GO  TO  EXECDTE  MEND  -  ENTER  2’ 

VRITE(6,*)’  DRAW  CDRRENT  ROBOT  -  ENTER  O’ 

VRITE(6,*)’  QDIT  -  ENTER  10’ 

READ*, OPTION 
WRITE (6,*) 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

C  SETDP  MEND 
C 

C  SET  SETDP  MEND  SELECTION  VARIABLE 

C 

IF  (OPTION  .EQ.  1)  THEN 

157  VRITE(6,*) ’CHANGE  VIEWPOINT,  INPDT  -  1’ 

WRITE(6,*) ’CHANGE  FOCDS,AND  MAGNITDDE,  INPDT  -  3’ 
WRITE(6,*) ’CHANGE  SYSTEMS  DRAWING,  INPDT  -  4’ 

WRITE (6,*) ’CHANGE  FIXED  HAND  POSITION,  INPDT  -  6’ 
WRITE(6,*) ’REPOSITION  ROBOT,  INPDT-  7’ 

WRITE(6,*) ’CHANGE  ACTIVE  ROBOT  INPDT  -  8’ 

WRITE(6,*)’RETDRN  TO  MAIN  MEND,  INPDT  -  9’ 

WRITE(6,*) ’DRAW  ROBOT  INPDT  -  0’ 

WRITE(6,*)’QDIT  INPDT  -  10’ 

READ*, SET 
WRITE(6,*) 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
C  VIEWPOINT 
C 

C  X,Y,Z  -  THE  VECTOR  TO  THE  VIEWPOINT 
C 

IF  (SET  .EQ.  1)  THEN 
WRITE(6,*) 

WRITE(6,*)’0LD  VIEWPOINT  -  X,Y,Z’ 

WRITE(6,190)X,Y,Z 

190  FORMAT(’  S3F6.0,’  INCHES’) 

WRITE(6,*) ’INPDT  -  NEW  VALDES  -  X,Y,Z  IN  INCHES’ 

READ*,X,Y,Z 

WRITE (6,*) 

C  THE  FOLLOWING  CODE  DETERMINES  THE  ORIENTATION  OF  THE  ROOM  AND 
C  EVERYTHIN^  INSIDE. 

C 

C  FIRST  DETERMINE  THE  PITCH  OF  THE  VIEWPOINT 
C 

199  IF  (Z  .EQ.  0)  THEN 

IF  (X  .EQ.  0)  THEN 
GO(2,2,1,1)=0 
ELSE  IF  (X  .LT.  0)  THEN 
G0(2,2,l,l)=h/2 

ELSE 

G0(2,2,l,l)=- PI/2 
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END  IF 
ELSE 

G0(2,2,1,1)=-ATAN(X/ABS(Z)) 
END  IF 

IF  (Z  .LT.  0)  THEN 

G0(2,2,1,1)=PI  -  G0(2,2,l,l) 
END  IF 


ROTATE  ROOM  FOR  A  SIDE  (OR  OTHER)  VIEV 

IF  ((X  .EQ.  0)  .AND.  (Z  .EQ.  0))  THEN 
IF  (Y  .LT.  0)  THEN 

G0(l,2,l,l)=-PI/2 

ELSE 

G0(l,2,l,l)=PI/2 
END  IF 

ELSE 

G0(1,2,1,1)=ATAN(Y/SQRT((X*X)+(Z*Z))) 

END  IF 

DISTANCE  FROM  VIEWPOINT  TO  FOCAL  POINT (WHICH  IS  INITIALLY  [0,0,0]) 

ZPOINT=SQRT(X*X+Y*Y+Z*Z) 

IF  (INIT  .EQ.  1)  THEN 
GO  TO  119 

ELSE  IF  (INIT  .EQ.  2)  THEN 
GO  TO  502 

END  IF 
WRITE (6,*) 

GO  TO  157 


FOCDS  AND  MAGNITUDE 


FOCUS  TWO  DIMENSIONAL  SCREEN  FOCAL  POINT  FOR  VIEWING 
X  IS  POSITIVE  TO  THE  RIGHT 
Y  IS  POSITIVE  UPWARDS 


MAG  MAGNIFICATION  VALUE 


C 

C 


CHES’ 


ELSE  IF  (SET  .EQ.  3)  THEN 
WRITE(6,’^)’  OLD  FOCUS  X,Y  IN  INCHES’ 
WRITE(6,259)  (FOCUS(I) ,1=1 ,2) 

259  FORMAT (’  ’,2F6.0,’  INCHES 

WRITE(6,*)’  INPUT  NEW  FOCUS  IN 
READ  ^,(F0CUS(I),I=1,2) 

WRITE(6,*)’  OLD  MAGNIFICATION’ 
WRITE(6,264)  MAG 
264  FORMAT (’  ’,F6.0) 

WRITE(6,*)’  INPUT  NEW  MAGNIFICATION 
READ  ^,MAG 


WRITE(6,*) 

GO  TO  157 

SYTEMS  DRAWING  MEND 

-  CAUSES  THE  SYSTEMS  TO  BE  OR  NOT  TO  BE  DRAWN 
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ENTER  1’ 


ELSE  IF  (SET  .EQ.  4)  THEN 
270  IP  (DRAV(l)  .EQV.  .TRUE.)  THEN 
VRITE(6,*)’D0  NOT  DRAW  ROOM, 

ELSE 

WRITE (6,*) ’DRAW  ROOM,  ENTER  1’ 

END  IF 

IF  (DRAV(2)  .EqV.  .TRUE.)  THEN 

VRITE(6,*)’D0  NOT  DRAW  LEFTARM  MERLIN,  ENTER  2’ 

ELSE 

VRITE(6,*)’DRAV  LEFTARM  MERLIN,  ENTER  2’ 

END  IF 

IF  (DRAV(3)  .EQV.  .TRUE.)  THEN 

VRITE(6,*)’D0  NOT  DRAW  RIGHTARM  MERLIN,  ENTER  3’ 

ELSE 

WRITE ( 6 , * ) ’ DRAW  RIGHTARM  MERLIN ,  ENTER  3 ’ 

END  IP 

IF  (DRAW(4)  .EQV.  .TRUE.)  THEN 

WRITE(6,*)’D0  NOT  DRAW  LEFTHAND  UTAH,  ENTER  4’ 

ELSE 

WRITE(6,*)’DRAW  LEFTHAND  UTAH,  ENTER  4’ 

END  IF 

IF  (DRAW(5)  .EQV.  .TRUE.)  THEN 

WRITE(6,*)’D0  NOT  DRAW  RIGHTHAND  UTAH,  ENTER  5’ 

ELSE 

WRITE(6,*)’DRAW  RIGHTHAND  UTAH,  ENTER  5’ 

END  IF 

WRITE(6,*)’T0  qUIT,  ENTER  6’ 

READ  *,N 

IF  (DRAW(N)  .EqV.  .FALSE.)  THEN 
DRAV(N)  =  .TRUE. 

ELSE 

DRAW(N)  =  .FALSE. 

END  IF 

IF  (N  .Eq.  6)  THEN 
GOTO  157 
END  IF 
GOTO  270 
POSITION  HAND 

ANG, ANGLE  TEMPORARY  STORAGE  OF  ANGLES  FOR  READING  INPUT 
ELSE  IF  (SET  .Eq.  6)  THEN 

WRITE(6,=^)’  YOU  ARE  GOING  TO  BE  ASKED  FOR  PITCH, 

$  YAW,  AND  ROLL,’ 

WRITE(6,*)’  AND  THEN  X,Y,  AND  Z  OFFSET  FOR  THE  UTAH  HAND’ 
WRITE(6,*)’  WITH  RESPECT  TO  THE  CENTER  END  OF  THE  ARM’ 

C  THE  YAW, PITCH,  AND  ROLL  ANGLES  ARE  APPLIED  TO  A  BLANK  INITIAL 
C  VECTOR  MATRIX  ON  THE  HAND,  AS  IS  THE  OFFSET  VECTOR 
C  YAW 
C 

VRITE(6,302)  GO(l ,2,2,HANDNUM(ACTIVE))/PI*180 
302  FORMAT(’  CURRENT  VAV  ANGLE  IS’,F8.2,^  DEGREES’) 

WRITE(6,*) ’INPUT  NEW  YAW  ANGLE  IN  DEGREES’ 

READ*, ANGLE 
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GO ( 1 , 2 , 2 , HANDNUM (ACTIVE) ) =ANGLE/ 180*PI 
C  PITCH 

VRITE(6 ,308)  GO (2 ,2 ,2 ,HANDNUI(ACTIVE) ) /PI*180 
308  FORMATS  CDRRENT  PITCH  ANGLE  IS’,F8.2,’  DEGREES’) 

VRITE(6,*) ’INPUT  NEV  PITCH  ANGLE  IN  DEGREES’ 

READ*, ANGLE 

GO (2 , 2 , 2 , HANDNDM (ACTIVE) ) =ANGLE / 180*PI 
C  ROLL 

WRITE (6 ,314)  GO (3 , 2 , 2 , HANDNUM(ACTIVE) ) /PI*180 
314  FORMAT (’  CURRENT  ROLL  ANGLE  IS’,F8.2,’  DEGREES’) 

VRITE(6,*) ’INPUT  NEV  ROLL  ANGLE  IN  DEGREES’ 

READ*, ANGLE 

GO (3 , 2 , 2 , HANDNUM (ACTIVE) ) =ANGLE/180*PI 
OFFSET 

XH,YH,ZH  HAND  POSITIONING  VARABLES 

VRITE(6,*)’X,Y,Z  OFFSET  FOR  THE  UTAH  HAND  IS  MEASURED’ 
VRITE(6,*)’FR0M  THE  CENTER  END  OF  THE  ROBOT  WRIST  PIN’ 
VRITE(6,*) ’THESE  VALUES  ARE  MEASURED  WITH  RESPECT  TO’ 
VRITE(6,*)’  THE  LAST  COORDINATE  SYSTEM  OF  THE  MERLIN’ 
VRITE(6,327)XH,YH,ZH 

327  FORMAT (’  CURRENT  X,Y,Z  OFFSET  IS  ’,3F8.2,’  INCHES’) 

WRITE (6,*)’  ENTER  HAND  OFFSET  X,Y,Z  IN  INCHES’ 

READ  XH,YH,ZH 
GO ( 1 , 1 , 2 , HANDNUM ( ACTIVE) ) =XH 
GO (2 , 1 , 2 , HANDNUM (ACTIVE) ) =YH 
G0(3 , 1 , 2 ,HANDNUM( ACTIVE) ) =ZH 
VRITE(6,*) 

GO  TO  157 
REPOSITION  ROBOT 

XP,YP,ZP  ROBOT  POSITIONING  VALUES 

ELSE  IF  (SET  7)  THEN 

VRITE(6,*) ’ENTER  REPOSITION  POINT  OF  MERLIN  ROBOT’ 

VRITE(6,*) ’POSITIVE  X  IS  TO  THE  RIGHT’ 

VRITE(6,*) ’POSITIVE  Y  IS  UP’ 

VRITE(6,*) ’POSITIVE  Z  IS  OUT  OF  THE  SCREEN’ 

VRITE(6,*)’THE  ORIGIN  IS  THE  LOVER, BACK, LEFT  CORNER  OF  ROOM’ 
WRITE (6,*) ’THE  OLD  X,Y,Z  POSITION  IS  ’ 

WRITE (6 ,347)60(1,1,2, MERLNDM( ACTIVE) ) , GO (2 , 1 , 2 , 

$  MMLNUMf  ACTIVE) ) ,  GO  (3 , 1 , 2  ,MERLNDM(  ACTIVE) ) 

347  FORMAT (3F8. 2,’  INCHES’) 

VRITE(6,*) ’ENTER  PLACEMENT  OF  BOTTOM  CENTER  OF  ROBOT-’, 

$  h,Y,Z  IN  INCHES’ 

READ*,XP,YP,ZP 

GO ( 1 , 1 , 2 , MERLNUM (ACTIVE) ) =XP 
GO (2 , 1 , 2 , MERLNUM ( ACTIVE) ) =YP 
GO (3 , 1 , 2 , MERLNUM (ACTIVE) ) =ZP 
WRITE (6,^) 

WRITE (6,*  ’THE  BASE  OF  THE  ROBOT  MAY  BE  ROTATED.’ 
VRITE(6,*)’THE  CURRENT  ROTATION  IN  DEGREES  IS’ 

VRITE(6,*)  GO(2,2,2,MERLNUM(ACTIVE))*180/PI,’  DEGREES’ 
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VRITE(6,*) ’ENTER  BASE  ROTATION  IN  DEGREES’ 

READ*,N 

GO (2 , 2 , 2 ,MERLNDM( ACTIVE) ) =N/180*PI 
GOTO  157 

C  CHANGE  ACTIVE  ROBOT 

ELSE  IF  (  SET  .EQ.  8)  THEN 
IF  (ACTIVE  .EQ.  1)  THEN 
ACTIVE  =  2 

ELSE 

ACTIVE  =  1 
END  IF 

VRITE(6,*) ’ACTIVE  ROBOT  IS  ’, ACTIVE 
VRITE(6,*) 

GOTO  157 
C  RETURN 

ELSE  IF  (SET  .EQ.  9)  THEN 
GOTO  143 

C  REDRAW 

ELSE  IF  (SET  .EQ.  0)  THEN 
GOTO  587 

C  QUIT 

ELSE  IF  (SET  .EQ.  10)  THEN 
GOTO  656 

ELSE 

GO  TO  157 
END  IF 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
C  EXECUTE  MENU 
C 

C  EXECUTE  MENU  SELECTION  VARIABLE 

C 

ELSE  IF  (OPTION  .EQ.  2)  THEN 

374  VRITE(6,*) ’MOVE  ROBOT  INDIVIDUAL  JOINTS,  INPUT  -  1’ 
VRITE(6,*)’M0VE  INDIVIDUAL  FINGER  JOINTS,  INPUT  -  2’ 

WRITE  6,*  ’SAVE  THIS  VIEW.  INPUT-  3’ 

WRITE(6,*)’DRAW  ROBOT  FROM  SAVED  DATA  FILE, INPUT  -  4’ 
WaiTE(6,*)’M0VE  ROBOT  TIP  POINT  TO  POINT,  INPUT  -  5’ 
WRITE(6,*)’M0YE  ROBOT- ALL  JOINTS  IN  STEPS,  INPUT  -  6’ 
WRITE(6,*) ’RETURN  TO  MAIN  MENU,  INPUT  -  9’ 

WRITE(6,*)’DRAW  ROBOT,  INPUT  -  0’ 

WRITE(6,*)’T0  QUIT,  INPUT  -10’ 

READ*, EXECUTE 
WRITE (6,*) 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
C  MOVE  ROBOT 
C 

C  C  DEFINES  ACTIVE  LINK  NUMBERS 
C 

C  LIMITS  DEFINES  THE  LIMITS  OF  EACH  JOINT  IN  DEGREES 
C 

DATA  C/4,5,7,8,9,10/ 

DATA  LIMITS/  147,-236,-236,-360,-90,-360,147,56,56,360,90,360/ 
IF  (EXECUTE  .EQ.  1)  THEN 
395  WRITE(6,*) ’CHOOSE  JOINT  FOR  CHANGE’ 
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VRITE(6,*)  ’VAIST  -  1’ 

VRITE(6,*)  ’SHOULDER  -  2’ 

VRITE(6,*)  ’ELBOV  -  3’ 

WRITE (6,*)  ’WRIST  ROLL  -  4’ 

WRITEfe,*)  ’WRIST  PITCH  -  5’ 

WRITE (6,*)  ’HAND  ROLL  -  6’ 

WRITE(6,*)  ’TO  EXIT  -  0’ 

READ  A 

IF  (A  .EQ.  0)  THEN 
GO  TO  374 

ELSE 


ANG  =  GO(3,2,C(A),MERLNUM(ACTIVE))*180/PI 

408  WRITE(6,409)  ANG 

409  FORMAT (’  OLD  VALUE  ’,F8.2,’  DEGREES’) 

WRITE(6,*)’  INPUT  -  NEW  VALUE  IN  DEGREES’ 

READ*,D 

IF  ((D  .LT.  LIMITS(A,1))  .OR. 

$  (D  .GT.  LIMITS(A,2)))  THEN 

WRITE(6,415)  LIMITS(A,1),LIMITS(A,2) 

415  FORMAT(’  ANGLE  MUST  BE  BETWEEN  \ 

$  F8.2,’  AND  ’,F8.2,’  DEGREES’) 

GO  TO  408 
END  IF 

G0(3,2,C(A),MERLNDM(ACTIVE))  =  D*PI/180 
GO  TO  395 
END  IF 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

MOVE  FINGERS 

CF  IS  THE  SYSTEM  VARIABLE 

AF  IS  THE  SYSTEM  COUNTER  VARIABLE 

BF  IS  THE  LINK  VARIABLE 

ELSE  IF  (EXECUTE  .EQ.  2)  THEN 
432  WRITE(6,*) 

WRITE(6,*) ’CHOOSE  JOINT  FOR  CHANGE’ 


WRITE(6,*)  ’OTH  FINGER  -  1’ 

WRITE(6,*)  ’1ST  FINGER  -  2’ 

WRITE(6,*)  ’2ND  FINGER  -  3’ 

WRITE(6,*)  ’THIRD  -  4’ 

WRITEfO,*)  ’TO  EXIT  -  0’ 

READ  AF 
WRITE(6,*) 

DO  443  1=1,4 


CF(I)=HANDNUM(ACTIVE)  +  I 
443  CONTINUE 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
C  CF  FINGER  JOINT  VARIABLE 
IF  (AF  .EQ.  0)  THEN 
GO  TO  374 
ELSE 

450  A  =  CF(AF) 
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6,* 

6,* 

6,* 


’CHOOSE  POSITION 
’1ST  JOINT  - 

’2ND  JOINT  - 

’3RD  JOINT  - 

’4TH  JOINT  - 

’TO  EXIT 


WRITE ( 

write! 
write! 

WRITE 
WRITE 

write! 

READ 
END  IF 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 


FOR  CHANGE’ 
1’ 

2’ 

3’ 

4’ 

O’ 


IF 
ELSE 


[BF  .Eq.  0) 
)0  TO  432 


THEN 


BF  =  BF+1 
WRITE(6,*) 

WRITE(6,466)(GO(3,2,BF,A))*180/PI 
466  FORMAT(’  OLD  VALDE  ’,F8.2,’  DEGREES’) 

WRITE(6,*)’  INPUT  -  NEW  VALUE  IN  DEGREES’ 

READ*,D 

G0(3,2,BF,A)  =  D*PI/180 
WRITE(6,*) 

GO  TO  450 
END  IF 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

c  SAVE  VIEW 

C  STORES  THE  VARIABLE  LINK  PARAMETERS  IN  AN  OUTPUT  FILE 

C 

ELSE  IF  (EXECUTE  .EQ.  3)  THEN 

OPENf UNIT-8, F1LE= ’SAVE. DAT’, STATDS=’NEV”) 

WRITE(8,*)  X,Y,Z,ZPOINT, MAG, FOCUS 
DO  484  I=1,R0BNUM 

DO  484  J=1,L1NKNUM(R0BNUM) 

WRITE  8,*)(G0(L,1,J,I),L=1,3) 
VRITE(8,*)(G0(L,2,J,I),L=1,3) 

484  CONTINUE 

CL0SE(UNIT-8) 

GO  TO  374 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
ELSE  IF  (EXECUTE  .EQ.  4)  THEN 

C  DRAW  FROM  THE  LAST  SAVED  VIEW 

C  CALLS  THE  VARIABLE  LINK  VALUES  FROM  AN  OUTFILE  AND  DRAWS  THE  ROBOT 
0PEN(UNIT-8, FILE- ’SAVE. DAT’, STATUS- ’OLD’) 

READ(8,*)  X,y,Z,ZPOINT, MAG, FOCUS 
DO  498  I-1,R0BNIM 

DO  498  J=1,LINKNUM(R0BNUM) 

READ(8,*)(G0(L,1,J,I),L-1,3) 

READ(8,*j(G0(L,2,J,l),L-l,3) 

498  CONTINUE 
INIT  -  2 
GO  TO  199 
502  INIT  =  0 

CLOSE(UNIT-H) 

GO  TO  587 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
ELSE  IF  (EXECUTE  .EQ.  5)  THEN 
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MOVE  TO  POINT 

B  STORAGE  OF  JOINT  ANGLES 


KINNUM  PARAMETER  PASSED  IN  CALL  INKIN 
KINNDM=1  (LEFT  HAND)  ,KINNIIM=2  (RIGHT  HAND) 


INKIN  PROVIDES  THE  ANGLES  TO  DRAV  TO  A  CERTAIN  POINT 
CALL  INKIN (B, KINNUM) 

DO  518  1=1,6 

G0(3,2,C(I), MERLNDM (ACTIVE) ) =B ( I ) / 180*PI 
518  CONTINUE 


GO  TO  587 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 


ELSE  IF  (EXECUTE  .EQ.  6)  THEN 
MOVE  BY  ANGLES 

NUM  NUMBER  READING  VARIABLE 

STEPS  NUMBER  OF  INCREMENTS  DURING  MOTION 

ERASE  LOGICAL  -  IS  SCREEN  ERASED  BETWEEN  VIEWS 

REPEAT  LOGICAL  FOR  INTERNAL  MEMORY  CONCERNING  DRAWING  SETUP 

533  WRITE (6,*) ’INPUT  THE  SIX  ANGLES, 

$  WAIST, SHOULDER, ELBOW, WRISTROLL,WRISTPITCH,HANDROLL) ’ 
WRITE(6,*) ’CURRENT  ANGLES  IN  DEGREES  ARE’ 

WRITE(6 , 36) (GO (3 , 2 , C(I) ,MERLNUM(ACTIVE) ) *180/PI ,1=1,6) 

36  F0RMAT(6F7.1) 

READ(6,*)(M2(I,1),I=1,6) 

WRITE(6,*)’D0  YOU  WANT  THE  SCREEN  ERASED  BETWEEN  STEPS’ 
WRITE(6,*) ’IF  YES  INPUT  1’ 

WRITE(6,*)’IF  NO  INPUT  O’ 

READ*, NUM 

IF  (NUM  .EQ.  1)  THEN 
ERASE  =  .TRUE. 

ELSE 

ERASE  =  .FALSE. 

END  IF 

DO  556  1=1,6 

IF  ((M2(I,1)  .LT.  LIMITS(I,1))  .OR. 

$  («2(I,1)  .GT.  LIMITS(I,2)))  THEN 

WRITE ( 6 , 552 )  I , LIMITS ( I , 1 ) , LIMITS ( I , 2 ) 

552  FORMAT (’ANGLE-  ’,12,’  MUST  BE  BETWEEN  ’, 

$  F7.1,’  AND  ’,F7.1,’  DEGREES’) 

GO  TO  533 
END  IF 

556  CONTINUE 
DO  560  1=1,6 

M2(I,1)=M2(I,1)/180*PI 

M1(I,1)=G0(3,2,C(I),MERLNUM(ACTIVE)) 

560  CONTINUE 
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ca  n  o  o  o  rj 


C  Ml  IS  THE  CIJHEENT  SET  OF  ANGLES,  12  IS  THE  FINAL  SET  (TO  BE  INPUT 
C  BY  USEH),  13  IS  RETURNED  FROl  GENAN6  AND  CONTAINS  THE  EQUALLY 
C  SPACED  INCREIENTAL  SET  OF  STEP  NUIBER  OF  ANGLES. 

C 

CALL  GENANG(11,12,6,1,13,STEPS) 

DO  573  J=l, STEPS 

IP  (J  .EQ.  STEPS)  THEN 
REPEAT  =  .FALSE. 

ELSE 

REPEAT  =  .TRUE. 

END  IF 

DO  570  1=1,6 

G0(3,2,C(I),1ERLNU1(ACTIVE))=13(I,1,J) 

570  CONTINUE 
GOTO  587 

572  CONTINUE 

573  CONTINUE 
GO  TO  374 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCGCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
C  RETURN  TO  IAIN 

ELSE  IF  (EXECUTE  .EQ.  9)  THEN 
GOTO  143 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
REDRAW 

ID  STORAGE  OF  TWO  DIIENSIONAL  DRAWING  CORNER  POINTS 

COUNT  COUNTER  FOR  LINK  DRAWING  FRAIES 

ELSE  IF  (EXECUTE  .EQ.  0)  THEN 
587  CALL  TRANSFILL ( GO, TR,1,LINKNU1(1),1) 

IF  (DRAW(l)  .EQV.  .TRUE.)  THEN 

CALL  XDFILL (XO , XD , TR , LINKNUl ( 1 ) , 1 , 

$  FOCUS, HAG, COUNT, ZPOINT) 

END  IF 

C  THE  TRANSFORIATION  lATRIX  OF  THE  LAST  LINK  ON  THE  lERLIN  HOST  BE 
C  ATTACHED  TO  THE  FIRST  TRANSFORIATION  HATRIX  ON  THE  HAND,  AND  THE 
C  FINGERS  HOST  BE  ATTACHED  TO  THE  HAND 
DO  592  L  =  2,3 

IF  (DRAW  L)  .EQV.  .TRUE.)  THEN 
CALL  ADDJN  (GO , TR , LINKMl , L ,  RECEIVE) 

CALL  TRANSFILL(G0,TR,2,LINKNUM(L) ,L) 

CALL  XDFILL(X0,XD,TR,LINKNU1(L) ,L, 

$  FOCUS, HAG, COUNT, ZPOINT) 

END  IF 

592  CONTINUE 

DO  598  N  =  4,5 

IF  (DRAW(N)  .EQV.  .TRUE.)  THEN 
DO  594  L=HANDNDl(N-3),(HANDNDl(N-3)+4) 

CALL  ADDJN  (GO ,  TR ,  LINKNUl ,  L ,  RECEIVE) 

CALL  TRANSFILL ( GO ,TR , 2 , LINKNUl (L) , L) 

CALL  XDFILL(XO,XD,TR,LINKNUH(L) ,1, 

$  FOCUS, 1AG,C0UNT,ZP0INT) 

594  CONTINUE 
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rsrsra  cnoorsrj  ocao 


END  IF 
598  CONTINUE 
604  CONTINUE 

IF  THERE  IS  ONLY  ONE  YIEV  -  DRAV  VIEV 

IF  (REPEAT  .EOV.  .FALSE.)  THEN 
CALL  DRAVBOT(XD, MONITOR, COUNT) 

COUNT  =  0 
ELSE 

IF  MORE  THAN  ONE  VIEV,  DOES  THE  USER  VANT  TO  ERASE 
BETWEEN  VIEWS?  IF  TO  BE  ERASED  SET  FLAG. (-999) 

ADVANCE  COUNT. 

IF  (ERASE  .EQV.  .TRUE.)  THEN 
COUNT  =  COUNT  +  1 
XD(1,1, COUNT)  =  -999 
END  IF 
END  IF 

RETURN  TO  SECTION  OF  PROGRAM  FROM  WHICH  REDRAW  WAS  CALLED 

IF  (OPTION  .EQ.  1)  THEN 
GO  TO  157 

ELSE  IF  (OPTION  .EQ.  2)  THEN 
IF  (EXECUTE  .EQ.  6)  THEN 
GO  TO  572 

ELSE 

GO  TO  374 
END  IF 

ELSE 

GO  TO  143 
END  IF 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
C  QUIT 

ELSE  IF  (EXECUTE  .EQ.  10)  THEN 
GOTO  656 
C 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
C  RETURN  TO  EXECUTE  MENU 
C 

ELSE 

GO  TO  374 
END  IF 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
ELSE  IF  (OPTION  .EQ.  0)  THEN 
GOTO  587 
C  QUIT 

ELSE  IF  (OPTION  .EQ.  10)  THEN 

GOTO  656 

ELSE 

GO  TO  143 
END  IF 
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C  END  OPTION 
656  CONTINUE 
STOP 
END 

C  END  OF  lilN  PROGRAM 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

c 

C  TR  STORES  TUNSFORl  MATRICES  FOR  All  LINKS 

C 

C  ADDJN  LINKS  THE  TRANSFORMATION  MATRICES  OF  SYSTEMS 

SUBROUTINE  ADD_ 

REAL  60(3,2,10, 

INTEGER  ROBNUM, 

DO  15  J=l,4 
DO  15  1=1,4 

TR(I , J , 1 , ROBNUM) =TR(I , J ,LINKNUM(RECEIVE(ROBNUM) ) , RECEIVE 
$  (ROBNUM)) 

15  CONTINUE 

CALL  TRANSFILL ( GO , TR , 2 , LINKNUM ( ROBNUM) , ROBNUM) 

RETURN 

END 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

C  TRANSFILL  FILLS  TRANSFORMATION  MATRIX 
C 


ON(GO,TR,LINKNUM,ROBNUM,RECEIVE) 

10),TR(4,4,10,10) 

LltaM(20),RECEIVE(20) 


SUBROUTINE  TRANSFILL ( GO , TR , BEG , END , R) 

REAL  GO(3,2,10,14),TR(4,4,10,14) 

INTEGER  BEG, END, R,ZPOINT, MONITOR 
DO  29  I=BEG,END 

J!  }’i’J’’^)=^“^(W,2,I,R)}*C0SjrG0{2,2,I,R)) 
TR  1,2,I,R  =-SlN?Gd(3,2,I,R  )*COS(Gd(iii,R 
TR(1,3,I,R)=SIN(G0(2,2,I,R))  ^  ^ 

TE(l,4,I,RUG0(l,l,I,Ri 

™  -  COS(00(3,2,I,R))*SIN(GO(2,2,I,R))* 


I) 


TM2,1,I,R, 
SIN(G0(1 


TR(2,4,I,R)=G0(2,1,I,R)  v  v 

TR(3,l,I,R)=-CdS(G0(3,2,I,R))*SIN(G0(2,2,I,R))* 

^  _„,,^^S(G0(1,2,I,R))+SIN(G0(3,2,I,R))*SIN(G0(1,2,I,R)) 

IR  4,1,1, R)=0 
TR(4,2,I,R)=0 
TR(4,3,I,R)=0 
TR(4,4,I,R)=1 

9Q  rnNTTNn]? 

CALL  TRANSFORM (TR,1, END, R) 

RETURN  ’  ’  / 

END 
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cccccccccccrcccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

c 

C  TRANSFORM  CALCULATES  THE  TRANSFORMATION  MATRII  BY  MATRIX 
C  MULTIPLICATION 
C 

SUBROUTINE  TRANSFORM (TR , SYS , END , R) 

DIMENSION  TR(4,4,10,14) ,MATA(4,4) ,MATB(4,4) ,MATC(4,4) 

REAL  TR,MATA,MATB,MATC 
INTEGER  SYS, END, R 
DO  23  I  =  (SYS+1),END 
DO  15  K  =  1,4 
DO  15  J  =  1,4 

MATA(J,K)  =  TR(J,K,(I-1),R) 

MATB(J,K)  =  TR(J,K,I,R) 

15  CONTINUE 

CALL  MULMATMAT(MATA,MATB,MATC) 

DO  22  K=l,4 
DO  22  J=l,4 

TR(J,K,I,R)  =  MATC(J,K) 

22  CONTINUE 

23  CONTINUE 
RETURN 
END 

C 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCGCCCCCCCCCCCC 

c 

C  XDFILL  FILLS  THE  ID  ARRAY  (8  I  2  X  COUNT)  WITH  DRAWING  POINTS 
C  POSITIONS 
C 

SUBROUTINE  XDFILL (XO , XD ,TR, END ,R , FOCUS , MAG , COUNT , ZPOINT) 
DIMENSION  X0(8,3,10,14) ,TR(4,4,10,14) ,MAT(4,4) , 

$  VEC(4),CORD(4),XD(8,2,500),FOCDS(2) 

REAL  XO, TR, MAT, VEC, CORD, X,Y,Z,XD,THX, THY, ZPOINT, FOCUS, MAG 
INTEGER  END, R, COUNT 
DO  43  L=1,END 

COUNT  =  COUNT  +  1 
DO  42  1=1,8 
DO  18  K=l,4 
DO  18  J=l,4 

MAT(J,K)  =  TR(J,K,L,R) 

18  CONTINUE 

DO  21  J=l,3 

VEC(J)=XO(I,J,L,R) 

21  CONTINUE 

VEC(4)=1.0 
C 

C  MULMATVEC  IS  A  MATRIX  OPERATION  OF  MULTIPLYING  THE  VECTOR  BY  THE 
C  TRANSFORMATION  MATRIX 
C 

CALL  MULMATVEC(MAT,VEC,CORD) 

C 

C  THE  SCREEN  VIEW  ALLOWS  A  20  DEGREE  VIEWING  TUNNEL 
C  THE  FOLLOWING  CODE  PLACES  THE  DRAWING  POINTS  IN  THEIR  RESPECTIVE 
C  PLACES  IN  THE  SCREEN  VIEW.  IF  THE  POINTS  LIE  OUTSIDE  THE 
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C  20  DEGREE  TUNNEL,  THEY  WILL  NOT  BE  DISPLAYED 
C 

I  =  CORD(l)  -  FOCDS(l) 

Y  =  C0RD(2)  -  F0CDS(2) 

Z  =  ZPOINT  -  CORD (3) 

THI=ATAN(X/Z) 

THY=ATAN(Y/Z' 

ID (1,1, COUNT)  =  50*THX/.349*MA6 
XD (I, 2, COUNT)  =  50*THY/.349*MAG 

42  CONTINUE 

43  CONTINUE 
RETURN 
END 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

c 

C  MULIATVEC  MULTIPLIES  A  4X4  MATRIX  VITH  A  4X1  VECTOR  OUTPUT  C(4X4) 

C 

SUBROUTINE  MULMATVEC(A,B,C) 

REAL  A(4,4),B(4),C(4),SUM 
DO  14  I  =  1,4 
SUM  =  0 

SDM^=  A(itj)*B(J)  +  SUM 

12  CONTINUE 
C(I)  =  SUM 

14  CONTINUE 
RETURN 
END 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 

c 

C  MULMATMAT  MULTIPLIES  TVO  4X4  MATRICES,  WITH  OUTPUT  C(4X4) 

C 

SUBROUTINE  MULMATMAT(A,B,C) 

REAL  A(4,4),B(4,4),C(4,4),SUM 
DO  16  I  =  1,4 
DO  15  J  =  1,4 
SUM  =  0 
DO  13  K  =  1,4 
SUM  =  A(I,K)*B(K,J)  +  SUM 

13  CONTINUE 
C(I,J)  =  SUM 

15  CONTINUE 

16  CONTINUE 
RETURN 
END 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 

c 

C  SUBROUTINE  GENANG  GENERATES  AN  ARRAY  OF  ANGLES. 

C  GIVEN  AN  INITIAL  SET  OF  ANGLES,  A  FINAL  SET  OF  ANGLES,  AND  THE 
C  NUMBER  OF  STEPS,  IT  COMPUTES  EQUALLY  SPACED  INTERMEDIATE  ANGLES, 

C  GOING  FROM  THE  INITIAL  TO  THE  FINAL  ANGLE. 

C 

SUBROUTINE  GENANG (Ml , M2 , N1 , N2 , M3 , STEPS ) 

REAL  Ml(6,4) ,M2(6,4) ,M3(6,4,200) ,DELM(6,4) 


oora  rara<^  rtc-itri 


INTEGER  N1,N2, STEPS 

VRITE(6*)’ INPUT  -  NUMBER  OF  STEPS’ 

READ (6,^)  STEPS 
DO  11  J=1,N2 
DO  11  1=1, N1 

DELM(I,J)=(M2(I,J)-M1(I,J))/STEPS 
11  CONTINUE 

DO  15  K=l, STEPS 
DO  15  J=1,N2 

DO  15  1=1, N1 

M3(I,J,K)=M1(I,J)+DELM(I,J)*K 

15  CONTINUE 
RETURN 
END 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

c 

C  SUBROUTINE  DRAVBOT  CONTAINS  THE  DISSPLA  SETUP  CALLS. 

C  IT  RECEIVES  THE  TVO  DIMENSIONAL  SCREEN  DRAWING  POINTS. 

C  EACH  SET  OF  EIGHT  POINTS  OF  A  LINK  ARE  CONNECTED  IN  A  CERTAIN 
C  SEQUENCE  TO  REPRESENT  A  SIX  SIDED  CLOSED  OBJECT  (THE  LINK^ 

C 

SUBROUTINE  DRAVBOT (ID , MONITOR , COUNT) 

REAL  XD(8,2,1000),X(16),Y(16) 

INTEGER  MONITOR, B( 16), COUNT 
DATA  B/1, 2, 4, 3, 7, 5, 6, 2, 6, 8,4, 8, 7, 5, 1,3/ 

SYSTEM  MODULE  CALL 

IF  (MONITOR  .EQ.  1)  THEN 

CALL  TEKALL(4010, 960, 0,1,0) 

ELSE  IF  (MONITOR  .EQ.  2)  THEN 
CALL  TK41(4107) 

ELSE 

CALL  REGIS(3,0) 

ENDIF 

THE  FOLLOVING  ARE  DISSPLA  DRAWING  COMMANDS 

CALL  BLOWUP (1.0) 

CALL  NOCHEK 
CALL  PAGE(11. 0,11.0) 

CALL  AREA2D(10.5,10.5) 

CALL  GRAF (-50.0,’ SCALE ’,50.0,-50.0,’ SCALE ’,50.0) 

CALL  INTAXS 
CALL  SCLPIC(.Ol) 

DO  51  1=1, COUNT 

IF  FLAG  (XD( 1,1, COUNT) =-999)  ERASE  SCREEN  AND  DRAW  NEXT  VIEW 

IF  (XD(1,1,I)  .EQ.  -999)  THEN 
CALL  ENDPL(O) 

CALL  PAGE(11. 0,11.0) 

CALL  AREA2D(10.5,10.5) 

CALL  GRAF (- 50.0,’ SCALE ’,50.0,-50.0,’ SCALE ’,50.0) 
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CALL  INTAXS 
CALL  SCLPIC(.Ol) 

ELSE 

DO  45  J=l,16 
C 

C  FILL  THE  DRAWING  ARRAY  IN  A  SEQUENCE  THAT  RESULTS  IN  DRAWING 
C  A  CUBE  WHEN  THE  CONSECUTIVE  POINTS  ARE  CONNECTED  WITH  LINES 


C  DRAW  THE  LINK. 

C 

CALL  CURVE (X,Y, 16,1) 

ENDIF 

51  CONTINUE 

CALL  ENDPL(O) 

CALL  DONEPL 

RETU.iN 

END 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 


PROGRAM  INKIN. FOR 

ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

c  MERLIN  ROBOT  INVERSE  KINEMATICS  PROGRAM 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
SUBROUTINE  INKIN(ANG,HAND) 

C  DEFINE  REAL  t  INTEGER  VALUES 

INTEGER  FLAG,WSPACE,HAND,SET 

REAL  T (4 , 4) , T1 , T2P1 , T2P2 , T3P , T3N , T4A , T4B , T5A , T5B , T6A , 
$T6B,A2,D2,D3,D4,D6,PI,WP,S1,S2,EP,EN,WR1,WR2,WR3,WR4, 
$WP1,WP2,WP3,WP4,HR1,HR2,HR3,HR4,DUM,Z(4,7),TPP,TPN,ANG(1,6) 

1  PRINT  ’ 

C  VALUE  OF  CONSTANTS 

PI  =  3.141592653589792 

C  SETUP  KINEMATIC  PARAMETERS  FOR  THE  MERLIN  6500  50  LB.  ROBOT 
C 

C  A2  IS  THE  DISTANCE  BETWEEN  SHOULDER  JOINT  AND  ELBOW  JOINT 
A2  =  17.38 

C  D4  IS  THE  DISTANCE  FROM  ELBOW  JOINT  TO  WRIST  PIN 
D4  =  17.24 

C  D6  IS  THE  DISTANCE  FROM  WRIST  PIN  TO  TIP  OF  THE  END- EFFECTOR 
D6  =  3.5 

C  SET  DP  D2  AND  D3.  D2  IS  THE  DISTANCE  FROM  THE  WAIST  VERTICAL 
C  AXIS  TO  THE  CENTER  OF  THE  UPPER  ARM.  D3  IS  THE  DISTANCE  FROM 
C  THE  CENTER  OF  THE  UPPER  ARM  TO  THE  CENTER  OF  THE  LOWER  ARM. 

O  l?nn  r  v*  »  vn 

V  ▲  uu  liiji.  i.  UiiAil/* 

IF(HAND  .EQ.  1)  THEN 


163 


D2  =  19.00 
D3  =  -7.00 
ELSE 

C  SET  UP  D2  AND  D3  FOR  THE  RIGHT  HAND. 

D2  =  -19.00 
D3  =  7.00 
ENDIF 

C  INITIALIZE  ALL  GLOBAL  VARIABLES  (  RETURNED  VARIABLES  ARE 
C  INITIALIZED  INSIDE  THE  SUBROUTINE  ONLY  ) 

VP  =  0.0 

51  =  0.0 

52  =  0.0 
EP  =  0.0 
EN  =  0.0 
VRl  =  0.0 
VR2  =  0.0 
VR3  =  0.0 
VR4  =  0.0 
VPl  =  0.0 
VP2  =  0.0 
VP3  =  0.0 
VP4  =  0.0 
HRl  =  0.0 
HR2  =  0.0 
HR3  =  0.0 
HR4  =  0.0 
DUM  =  0.0 
T2P1  =  0.0 
T2P2  =  0.0 

C  INITIALIZE  [  Z  ]  MATRIX 

C  THE  FIRST  COLUMN  OF  THE  MATRIX  IS  A  FLAG  FOR  VALIDITY  OF  THE 
C  SET  OF  JOINT  ANGLES  BEING  COMPUTED  BEING  ALL  VITHIN  THEIR  RANGES. 
C  THE  REMAINING  4X6  MATRIX  IS  USED  TO  STORE  THE  RESULTS  OF 
C  THE  COMPUTATIONS  IN  THE  ORDER  --  VAIST,  SHOULDER,  ELBOV, 

C  VRIST  ROLL,  VRIST  PITCH,  HAND  ROLL 
DO  2  I  =  1,4 
DO  2  J  =  1,7 
Z(I,J)  =  0,0 

2  CONTINUE 

C  ENTER  POSITION  AND  ORIENTATION  MATRIX  FROM  DATAFILE  OR  SCREEN 

3  CALL  MATENTER(T,D6) 

C  FLAG  SET  UP  FOR  END  POSITION  IN/OUT  OF  WORKSPACE 
C  VSPACE  =  0  IF  THE  END- EFFECTOR  IS  INSIDE  THE  WORKSPACE 
C  VSPACE  =  1  IF  THE  END- EFFECTOR  IS  OUTSIDE  THE  WORKSPACE 
C  SET  DEFAULT  VSPACE  FLAG  =  0 
VSPACE  =  0 

C  COMPUTE  VAIST  ANGLES  Tl. 

C  IN  THE  CALL  STATEMENT  BELOV,  T  IS  THE  4  X  4  POSITION  AND 
C  ORIENTATION  WORKSPACE,  Tl  IS  THE  COMPUTED  VAIST  ANGLE. 

CALL  VAIST(T,T1,D2,D3, VSPACE) 

C  IF  POSITION  DESIRED  AS  END-POINT  IS  OUTSIDE  THE  WORKSPACE 
C  GET  A  NEV  SET  OF  ENDPOINTS  FROM  THE  USER, 

IF (VSPACE  .EQ.  1)  THEN 
GOTO  3 
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ENDIF 


C  CONVERT  COIPflED  VAIST  ANGLE  FROM  RADIANS  TO  DEGREES. 

C  A  DDMIY  IS  USED  HERE  (DDM),  SINCE  ONLY  ONE  VAIST  ANGLE  EXISTS. 
CALL  RADEG(T1,0.0,VP,DDM) 

C  STORE  RESULTS  OF  VAIST  IN  [Z]  MATRIX  (SECOND  COLUMN) 

DO  5  I  =  1,4  ^ 

Z(I,2)  =  VP 

5  CONTINUE 

C  RESET  THE  VSPACE  FLAG  TO  0  FOR  THE  ELBOV  COMPUTATIONS. 

VSPACE  =  0 

C  COMPOTE  ELBOV  ANGLES  T3P,T3N 

CALL  ELBOV (T , T3P , T3N , A2 , D2 , D3 , D4 , VSPACE) 

C  IF  USER  DEFINED  END  POSITION  AND  ORIENTATION  IS  OUTSIDE 

C  THE  VORKSPACE,  RE-ENTRY  OF  MATRIX 
IF (VSPACE  .EQ.  1)  THEN 
GOTO  3 
ENDIF 

C  CONVERT  ELBOV  ANGLES  FROM  RADIANS  TO  DEGREES 
CALL  RADE6(T3P,T3N,EP,EN) 

C  STORE  RESULTS  OF  ELBOV  ANGLE  SOLUTION  IN  THE  FOURTH  COLUMN 

C  OF  MATRIX  [Z] 

DO  6  I  =  1,2 
Z(I,4)  =  EP 
Z(l+2,4)  =  EN 

6  CONTINUE 

C  COMPOTE  (  SHOULDER  t  ELBOV  )  ANGLES  TPP,TPN 
CALL  SH0ULDER(T,A2,D4,T1,T3P,T3N,TPP,TPN) 

C  COMPUTE  SHOULDER  ANGLES  T2P1,T2P2 
T2P1  =  TPP  -  T3P 
T2P2  =  TPN  -  T3N 

C  CONVERT  SHOULDER  ANGLES  FROM  RADIANS  TO  DEGREES 
CALL  RADEG(T2P1,T2P2,S1,S2) 

C  STORE  RESULTS  OF  SHOULDER  ANGLES  IN  [Z]  MATRIX  (THIRD  COLUMN) 
DO  7  I  =  1,2  ’ 

Z(I,3)  =  SI 
Z(I+2,3)  =  S2 

7  CONTINUE 


C 

C 

C 


C 


C 

C 


COMPUTE  VRIST  ROLL  ANGLES 

CALL  VR0LL(T,T4P1,T4P2,T1,TPP,TPN) 
CONVERT  VRIST  ROLL  ANGLES  FROM  RADIANS  TO 
CALL  RADEG(T4P1,T4P2,VR1,VR2) 

COMPUTE  ’VRIST  FLIPPED’  SOLUTIONS 
VR3  =  VRl  +  180.0 
VR4  =  VR2  +  180.0 

STORE  RESULTS  OF  VRIST  ROLL  IN  [Zl  MATRIX 
Z(l,5)  =  VRl 
Z(2,5)  =  VR3 
Z(3,5)  =  VR2 
Z(4,5)  =  VR4 

COMPUTE  VRIST  PITCH  ANGLES 


DEGREES 


(FIFTH  COLUMN) 


CALL  VPITCH(T,T5P1,T5P2,T1,TPP,TPN,T4P1,T4P2) 
CONVERT  VRIST  PITCH  ANGLES  FROM  RADIANS  TO  DEGREES 
CALL  RADEG(T5P1,T5P2,VP1,VP2) 
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C  COMPOTE  ’REVERSED  PITCH’  SOLUTIONS 
VP3  =  -  VPl 
VP4  =  -  VP2 

C  STORE  RESULTS  IN  [  Z  ]  MATRIX  -  SIXTH  COLUMN 
Z(l,6)  =  VPl 
Z(2,6)  =  VPS 
Z(3,6)  =  VP2 
Z(4,6)  =  VP4 
C  COMPUTE  MND  ROLL 

CALL  HROLL (T ,T6P1 ,T6P2 ,T1 ,TPP ,TPN ,T4P1 ,T4P2 ,T5P1 ,T5P2) 

C  CONVERT  HAND  ROLL  ANGLES  FROM  RADIANS  TO  DEGREES 
CALL  RADEG(T6P1,T6P2,HR1,HR2) 

C  COMPUTE  ’HAND  FLIPPED’  SOLUTIONS  FOR  HAND  ROLL 
HRS  =  HRl  +  180.0 
HR4  =  HR2  +  180.0 

C  STORE  RESULTS  IN  THE  [Z]  MATRIX  (SEVENTH  COLUMN) 

Z(l,7)  =  HRl 
Z(2,7)  =  HR3 
Z(3,7)  =  HR2 
Z(4,7)  =  HR4 

C  NORMALIZE  THE  COMPUTED  RESULTS. 

CALL  NORMAL(Z) 

C  CHECK  FOR  VALIDITY  OF  EACH  SOLUTION  SET. 

CALL  VALID(Z) 

C  PRINT  OUT  VALID  RESULTS  (  VALID  IF  VITHIN  JOINT  ANGLE  RANGE  ) 
PRINT  THE  VALID  INVERSE  KINEMATICS  RESULTS  ARE  :==  ’ 

DO  51  I  =  1,4 
IF(Z(I,1)  .EQ.  0.0)  THEN 
VRm(5,*)  ’THE  nilD  SOLUTION  NUMBER  IS 
VRITE  5,*)  (Z(I,J),J=2,7) 

ENDIF 

51  CONTINUE 

C  SET  DEFAULT  FOR  USER  CHOSEN  SET  OF  RESULTS  =  1 
SET  =  1 

C  qUERY  USER  FOR  CHOICE  OF  SET  OF  RESULTS  FROM  VALID  SET. 

PRINT  ENTER  YOUR  CHOICE  OF  ANGLES  (AS  A  SET)  ’ 

PRINT  FROM  THE  DIFFERENT  SETS  ABOVE  ’ 

READ (5, 52)  SET 

52  FORMAT(I) 

C  COPY  CHOSEN  SET  OF  RESULTS  TO  VECTOR  ANG(1,6) 

DO  53  J  =  1,6 
ANG(1,J)  =  Z(SET,J+1) 

53  CONTINUE 
RETURN 
END 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

C  POSITION  AND  ORIENTATION  DATA  ENTRY  ROUTINE 
SUBROUTINE  MATENTER(A,D6) 

INTEGER  MATCH, TIP 

REAL  A(4,4),PX1,PY1,PZ1,PX,PY,PZ,D6 
C  INITIALIZE  LOCAL  VARIABLES 
PXl  =  0.0 
PYl  =  0.0 
PZl  =  0.0 
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PX  =  0.0 
PY  =  0.0 
PZ  =  0.0 

C  INITIALIZE  MATRIX  [A] 

100  DO  101  I  =  1,4 

DO  101  J  =  1,4 
A(I,J)  =  0.0 

101  CONTINUE 

C  DATA  ENTRY  OF  POSITION  AND  ORIENTATION  MATRIX 
DO  102  I  =  1,3 
DO  102  J  =  1,4 

PRINT  ENTER  TRANSFORM  MATRIX  ENTRY’, I, J 
READ(5,^)  A(I,J) 

102  CONTINUE 

C  ADJUST  ROV  4  ENTRIES  TO  PREVENT  ENTRY  ERROR 
A(4,l)  =  0.0 
A(4,2)  =  0.0 
A(4,3)  =  0.0 
A(4,4)  =  1.0 

C  PRINT  OUT  MATRIX  TO  SCREEN 
PRINT  ’ 

PRINT  THIS  IS  THE  ENTERED  TRANSFORM  MATRIX.  ’ 

CALL  AOUT(A) 

PRINT  IF  YOU  WANT  TO  CHANGE  THE  MATRIX,  ENTER  0  ’ 

PRINT  * , ’  IF  POSITION  ENTRIES  REFER  TO  THE  TIP  OF  THE  ’ 

PRINT  END  EFFECTOR  -  ENTER  1  ’ 

PRINT  * , ’  IF  POSITION  ENTRIES  ARE  VITH  RESPECT  TO  THE  ’ 

PRINT  VRIST  PIN  .  ENTER  2  ’ 

READ (5, 104)  TIP 

104  FORMAT(I) 

C  ALLOW  FOR  CHANGE  OF  TRANSFORM  MATRIX  ENTRIES 
IF(TIP  .Eq.  0)  THEN 
GOTO  100 

C  ADJUST  END  EFFECTOR  POSITION  TO  VRIST  PIN  IF  POSITION  GIVEN  IS 

C  AT  THE  TIP  OF  THE  END- EFFECTOR 
ELSEIF(TIP  .EQ.  1)  THEN 

C  SETUP  POSITION  PARAMETERS  TO  END- EFFECTOR  TIP 
PXl  =  A(l,4) 

PYl  =  A(2,4) 

PZl  =  A(3,4) 

C  ADJUST  POSITION  PARAMETERS  TO  VRIST  PIN 
PX  =  PXl  -  D6  *  A(l,3) 

PY  =  PYl  -  D6  *  A(2,3) 

PZ  =  PZl  -  D6  *  A(3,3) 

C  RESET  POSITION  PARAMETERS  IN  [A]  MATRIX  TO  VRIST  PIN 
A(l,4)  =  PX 
A(2,4)  =  PY 
A(3,4)  -  PZ 
ENDIF 
RETURN 
END 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
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C  OUTPUT  OF  MATRIX  TO  SCREEN 
SUBROUTINE  AOUT(M) 

REAL  1(4,4) 

INTEGER  I,J 
DO  1001  I  =  1,4 
VRITE(5,*)  (M(I,J),J=1,4) 

1001  CONTINUE 
RETURN 
END 

ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

C  WAIST  \NGLE  COMPUTATION 

SUBROUTINE  VAIST(A, VI, X2,X3, SPACE) 

REAL  A(4,4),V1,X2,X3,RH0,PX,PY, TERM 1 , TERM2 , T2 , X23 
INTEGER  I, J, SPACE 

C  INITIALIZATION  OF  LOCAL  VARIABLES 
VI  =  0.0 
TERMl  =0.0 
TERM2  =0.0 
X23  =  0.0 
T2  =  0.0 

C  SET  UP  OF  POSITION  PARAMETERS 
PX  =  A(l,4) 

PY  =  A(2,4) 

C  COMPUTE  FIRST  TERM  FOR  VAIST  ANGLE  SOLUTION 
TERMl  =  ATAN2(PY,PX) 

C  COMPUTE  TERM2 

X23  =  (X2  +  X3) 

PXSq  =  PX  ♦  PX 
PYSQ  =  PY  *  PY 
PXPYSQ  =  pxsq  +  PYsq 
X23Sq  =  X23  *  X23 

C  IS  USER- SPECIFIED  POSITION  INSIDE  THE  WORKSPACE  ? 

C  SET  WORKSPACE  FLAG  TO  INSIDE  WORKSPACE 
SPACE  =  0 

IFfPXPYSq  .GT.  X23Sq)  THEN 

C  SPECIFIED  POSITION  IS  INSIDE  WORK- SPACE,  SO  COMPUTE  SECOND  TERM 
GOTO  301 
ELSE 

C  USER  SPECIFIED  POSITION  IS  OUTSIDE  WORKSPACE. 

C 

C  COMPUTE  DIFFERENCE  IN  TERMS 

ERROR  =  (ABS(PXPYSq  -  X23Sq)) 

C  IF  THE  COMPUTED  ERROR  <  0.0001,  THEN  COMPUTATIONAL  ERROR 

C  COULD  HAVE  CAUSED  THE  POSITION  TO  LIE  OUTSIDE  THE  WORKSPACE. 
IF(ERROR  .LT.  0.0001)  THEN 

C  YES,  COMPUTATIONAL  ERROR  OCCURED.  COMPUTE  T2,  FOLLOWED  BY 

C  THE  SECOND  TERM. 

T2  =  SqRT(ERROR) 

GOTO  302 
ELSE 

C  USER  SPECIFIED  POSITION  IS  DEIFINITELY  OUT  OF  WORKSPACE 
SPACE  =  1 

PRINT  OUTSIDE  WORKSPACE  ’ 

GOTO  303 
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ENDIF 

ENDIF 

301  T2  =  SQRT(PXPYSq  -  X23SQ) 

302  TER12  =  ATAN2(X23,T2) 

C  COMPUTE  SOLUTION  FOR  VAIST  ANGLE  VI 
VI  =  TERMl  -  TERM2 

303  RETURN 
END 

ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

C  ELBOV  ANGLE  DETERMINATION  ROUTINE 

SUBROUTINE  ELBOV (A , EP , EN , B2 , X2 , X3 , X4 , SPACE) 

INTEGER  SPACE 

REAL  A(4,4),EP,EN,B2,X2,X3,X4,KA,KB,X23,T1,T2P,T2N 
C  INITIALIZE  LOCAL  VARIABLES 
EP  =  0.0 
EN  =  0.0 
KA  =  0.0 
KB  =  0.0 
T1  =  0.0 
T2P  -  0.0 
T2N  =  0.0 
X23  =  0.0 

C  SET  UP  POSITION  PARAMETERS  OF  TRANSFORM  MATRIX 
PX  =  A(l,4) 

PY  =  A(2,4) 

PZ  =  A(3,4) 

C  COMPUTE  FIRST  TERM  OF  ARCTAN  FUNCTION 

TOO  -  ^  19  +  TO  ^ 

KA  =  -  (PX  *  PX)  -  (PY  *  PY)  -  (PZ  ♦  PZ) 

KB  =  (B2  *  B2)  +  (X23  *  X23)  +  (X4  *  X4) 

T1  =  (KA  +  KB)  /  (  2.0  *  B2  *  X4) 

Ttsq  =  T1  *  T1 

C  DETERMINE  IF  USER  DEFINED  POSITION  IS  OUTSIDE  VORKSPACE 
SPACE  =  0 

C  POSITION  IS  INSIDE  THE  VORK- SPACE  IF  TlSq  <  1.0 
IF(TlSq  .LE.  1.0)  THEN 
GOTO  401 
ELSE 

C  USER  DEFINED  POSITION  MAYBE  OUTSIDE  VORKSPACE 
C  THEREFORE,  COMPUTE  THE  ERROR 
ERROR  -  aBS(1.0  -  Tisq)) 

C  CHECK  TO  SEE  IF  COMPUTATIONAL  ERROR  COULD  HAVE  CAUSED  THE 
C  POSITION  TO  LIE  OUTSIDE  THE  VORKSPACE 
IF(ERROR  .LT.  0.0001)  THEN 
T2P  -  SqRT(ERROR) 

GOTO  402 
ELSE 

C  USER  ENTERED  POSITION  IS  OUTSIDE  VORKSPACE 
PRINT  OUTSIDE  VORKSPACE  ’ 

SPACE  =  1 
GOTO  403 
ENDIF 
ENDIF 

C  COMPUTE  SECOND  TERM  OF  ARCTAN  FUNCTION 
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401  T2P  =  SQETfl.O  -  TISQ) 

402  T2N  =  -  T2P 

C  COMPUTE  THE  TVO  POSSIBLE  SOLUTIONS  FOE  ELBOV  ANGLE  I.E.  EP  k  EN 
EP  =  ATAN2(T1,T2P) 

EN  =  ATAN2(T1,T2N) 

403  EETUEN 
END 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
C  SHOULDEE  +  ELBOV  ANGLE  DETEEMINATION  EOUTINE 

SUBEOUTINE  SHOULDEE (A , B2 , X4 , VP , EP , EN , APP , APN ) 

INTEGEE  I,J 

EEAL  A(4,4) ,B2,X4,VP,EP,EN,T1PP,T1PN,T2PP,T2PN,C1P,S1P 
$C3P , C3N , S3P , S3N , TIPPA , TlPNA , T2PPB , T2PNB , APP , APN 
C  INITIALIZE  LOCAL  VAEIABLES 
TIPP  =  0.0 
TIPN  =  0.0 
T2PP  =  0.0 
T2PN  =  0.0 
TIPPA  =0.0 
TlPNA  =0.0 
TIPP  =  0.0 
TIPN  =  0.0 
T2PPB  =0.0 
T2PNB  =0.0 
APP  =  0.0 
APN  =  0.0 

C  SETUP  OF  POSITION  PAEAMETEES 
PX  =  A(l,4) 

PY  =  A(2,4) 

PZ  =  A(3,4) 

C  COMPUTE  COSINE  AND  SINE  FUNCTION  VALUES  OF  THE  APPEOPEIATE  ANGLES 
CIP  =  COS(VP) 

SIP  =  SIN(VP) 

C3P  =  COS(EP) 

S3P  =  SiN(EP) 

C3N  =  COS(EN) 

S3N  =  SIN(EN) 

C  COMPOTE  ALL  POSSIBLE  FIEST  TEEMS  OF  AECTAN2  FUNCTION 
C 

C  VAIST  POSITIVE,  ELBOV  POSITIVE  (TIPP) 

TIPPA  =  B2  *  C3P  *  PZ 

TIPP  =  (((B2  *  S3P)  -  X4)  *  ((CIP  *  PX)  +  (SIP  ♦  PY)))  -  TIPPA 
C  VAIST  POSITIVE,  ELBOV  NEGATIVE  (TIPN) 

TlPNA  =  B2  *  C3N  *  PZ 

TIPN  =  (((B2  *  S3N)  -  X4)  ♦  ((CIP  *  PX)  +  (SIP  *  PY)))  -  TlPNA 
C  COMPUTE  ALL  POSSIBLE  SECOND  TEEMS  OF  AECTAN2  FUNCTION 
C 

C  VAIST  POSITIVE,  ELBOV  POSITIVE  (T2PP) 

T2PPB  =  ((B2  *  C3P)  *  ((CIP  *  ?X)  +  (SIP  *  PY))) 

T2PP  =  (((B2  *  S3P)  -  X4)  *  PZ)  +  T2PPB 

C  VAIST  POSITIVE,  ELBOV  NEGATIVE  (T2PN) 

T2PNB  =  ((B2  *  C3N)  *  ((CIP  *  PX)  +  (SIP  *  PY))) 

T2PN  =  ((  B2  *  S3N)  -  X4)  *  PZ)  +  T2PNB 

t  COMPOTE  ALL  FOUE  POSSIBLE  SOLUTIONS  OF  (THETA  2  +  THETA  3) 
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APP  =  ATAN2(T1PP,T2PP) 

APN  =  ATAN2(T1PN,T2PN) 

RETURN 

END 

ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

C  VRIST  ROLL  ANGLE  DETERMINATION  ROUTINE 

SUBROUTINE  VROLL (A , PPP , PPN , VP , T23PP , T23PN) 

INTEGER  PPPP,FPPN 

REAL  A(4,4) ,PPP,PPN,VP,T23PP,T23PN,T1P,R13,R23,R33, 

$S1P , CIP , C23PP , C23FN , S23PP ,S23PN , SNGCHI 

C  INITIALIZE  LOCAL  VARIABLES 
TIP  =  0.0 
T2PPP  =0.0 
T2PPN  =0.0 
PPP  =  0.0 
PPN  =  0.0 

C  SET  UP  SINGULARITY  CHECK  CONDITION 
SNGCHK  =  0.005 

C  SET  FLAGS  TO  NON- SINGULAR  CASE 
FPPP  =  0 
FPPN  =  0 

C  SETUP  MATRIX  ORIENTATION  PARAMETERS 
R13  =  A(l,3) 

R23  =  A(2,3) 

R33  =  A(3,3) 

C  SETUP  TRIG.  FUNCTIONS 
SIP  =  SIN(VP) 

CIP  =  COS(VP) 

C23PP  =  C0S(T23PP) 

S23PP  =  SIN(T23PP) 

C23PN  =  C0S(T23PN) 

S23PN  =  SIN(T23PN) 

C  COMPUTE  ALL  FIRST  TERMS  OF  ARCTAN2  FUNCTION 
TIP  =  -  (R13  *  SIP)  +  (R23  *  CIP) 

C  COMPUTE  ALL  SECOND  TERMS  OF  ARCTAN2  FUNCTION 

T2PPP  =  - (R13*C1P*C23PP)  -  (R23*S1P*C23PP)  +  (R33*S23PP) 

T2PPN  =  - (R13*C1P*C23PN)  -  (R23*S1P*C23PN)  +  (R33*S23PN) 

C  CHECK  FOR  SINGULARITY  CONDITIONS  AT  VRIST  PITCH 

IF((T1P  .LT.  SNGCHK  .AND.  TIP  .GT.  -  SNGCHK)  .AND. 

$(T2PPP  .LT.  SNGCHK  .AND.  T2PPP  .GT.-  SNGCHK))  THEN 
FPPP  =  1 
ENDIF 

IF((T1P  .LT.  SNGCHK  .AND.  TIP  .GT.  -  SNGCHK)  .AND. 

$(T2PPN  .LT.  SNGCHK  .AND.  T2PPN  .GT.  -  SNGCHK))  THEN 
FPPN  =  1 
ENDIF 

C  SET  VRIST  ROLL  TO  0.0  RADIANS  IF  SINGULARITY  DETECTED 

C  AT  VRIST  PITCH,  ELSE  COMPUTE  VRIST  ROLL.  NOTE  THAT  THIS  VILL 

C  CAUSE  THE  ROLL  TO  SHOV  UP  ONLY  IN  HAND  ROLL  ANGLE. 

C  SOLUTION  #  1 

IF(FPPP  .EQ.  1)  THEN 
PPP  =  0.0 
ELSE 

PPP  =  ATAN2(T1P,T2PPP) 


171 


ENDIF 

C  SOLUTION  #  2 

IF(FPPN  .EQ.  1)  THEN 
PPN  =  0.0 
ELSE 

PPN  =  ATAN2(T1P,T2PPN) 

ENDIF 

RETURN 

END 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 

C  WRIST  PITCH  DETERMINATION 

SUBROUTINE  VPITCH ( A , A5P1 , A5P2 , VP , APP , APN , V4P1 , V4P2) 

REAL  A(4,4) ,A5P1,A5P2,VP,APP,APN,V4P1,V4P2, 

STSAIPPPP , T5A2PPPP ,T5A3PPPP ,T5APPPP , T5A1PPNP , T5A2PPNP , T5A3PPNP , 
$T5APPNP ,T5B1PPPP,T5B2PPPP, T5B3PPPP , T5BPPPP , T5B IPPNP , T5B2PPNP , 
$T5B3PPNP , T5BPPNP , R13 , R23 , R33 , CIP , SIP , C23PP , S23PP , 

$C23PN , S23PN , C4P 1 , S4P 1 , C4P2 , S4P2 

C  INITIALIZE  LOCAL  VARIABLES  TO  0.0 
T5A1PPPP  =0.0 
T5A2PPPP  =0.0 
T5A3PPPP  =0.0 
T5APPPP  =0.0 
T5A1PPNP  =0.0 
T5A2PPNP  =0.0 
T5A3PPNP  =0.0 
T5APPNP  =0.0 
T5B1PPPP  =0.0 
T5B2PPPP  =0.0 
T5B3PPPP  =0.0 
T5BPPPP  =0.0 
T5B1PPNP  =0.0 
T5B2PPNP  =0.0 
T5B3PPNP  =0.0 
T5BPPNP  =0.0 
A5P1  =  0.0 
A5P2  =  0.0 

C  SETUP  ORIENTATION  PARAMETERS 
R13  =  A(l,3) 

R23  =  A(2,3) 

R33  =  A(3,3) 

C  SETUP  TRIG.  FUNCTIONS 
CIP  =  COS(VP) 

SIP  =  SIN(VP) 

C23PP  =  COS(APP) 

S23PP  =  SIN(APP) 

C23PN  =  COS(APN) 

S23PN  =  SIN(A?N) 

C4P1  =  C0S(V4P1) 

S4P1  =  SIN(V4P1) 

C4P2  =  C0S(V4P2) 

S4P2  =  SIN(V4P2) 

C  COMPOTE  FIRST  TERMS  OF  THE  ARCTAN2  FUNCTIONS 

T5A1PPPP  =  -  (R13  *  ((CIP  *  C23PP  *  C4P1)  +  (SIP  *  S4P1))) 
T5A2PPPP  =  -  (R23  ♦  ((SIP  *  C23PP  *  C4Pl)  -  (CIP  ♦  S4P1))) 
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T5A3PPPP  =  R33  *  S23PP  ♦  C4P1 

T5APPPP  =  T5A1PPPP  +  T5A2PPPP  +  T5A3PPPP 

T5A1PPNP  =  -  (il3  *  ((CIP  ♦  C23PN  *  C4P2)  +  (SIP  *  S4P2))) 

T5A2PPNP  =  -  (R23  *  ((SIP  *  C23PN  ♦  C4P2)  -  (ClP  *  S4P2))) 

T5A3PPNP  =  i33  ♦  S23PN  *  C4P2 

T5APPNP  =  T5A1PPNP  +  T5A2PPNP  +  T5A3PPNP 

C  COlPUTE  SECOND  TERMS  OF  THE  ARCTAN2  FUNCTIONS 
T5B1PPPP  =  -  (CIP  ♦  S23PP  ♦  R13) 

T5B2PPPP  =  -  (sip  ♦  S23PP  ♦  R23) 

T5B3PPPP  =  -  (C23PP  ♦  R33) 

T5BPPPP  =  T5B1PPPP  +  T5B2PPPP  +  T5B3PPPP 
T5B1PPNP  =  -  (Cl?  *  S23PN  *  R13) 

T5B2PPNP  =  -  (sip  ♦  S23PN  ♦  R23) 

T5B3PPNP  =  -  (C23PN  *  R33) 

T5BPPNP  =  T5B1PPNP  +  T5B2PPNP  +  T5B3PPNP 

C  COMPUTE  VRIST  PITCH  ANGIES  USING  ARCTAN2  FUNCTION 
A5P1  =  ATAN2(T5APPPP,T5BPPPP) 

A5P2  =  ATAN2(T5APPNP,T5BPPNP) 

RETURN 

END 

ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

C  DETERMINATION  OF  HAND  ROLL  ANGLES 

SUBROUTINE  HROLL (A , A6P1 , A6P2 , VP , APP , APN , A4P1 , A4P2 , A5P1 , A5P2) 
INTEGER  I,J 

REAL  A (4 , 4) , A6P1 , A6P2 , VP , APP , APN , A4P1 , A4P2 , A5P1 , A5P2 , 

SPPPPPA 1 , PPPPP A2 , PPPPP A3 , PPPPPA , PPNPPA 1 , PPNPPA2 , PPNPP A3 , PPNPP A 
SPPPPPB 1 , PPPPPB2 , PPPPPB3 , PPPPPB , PPNPPB 1 , PPNPPB2 , PPNPPB3 , PPNPPB 

C  INITIALIZE  LOCAL  VARIABLES  TO  0.0 
PPPPPAl  =0.0 
PPPPPA2  =0.0 
PPPPPA3  =0.0 
PPPPPA  =0.0 
PPNPPAl  =0.0 
PPNPPA2  =0.0 
PPNPPA3  =0.0 
PPNPPA  =0.0 
PPPPPBl  =  0.0 
PPPPPB2  =0.0 
PPPPPB3  =0.0 
PPPPPB  =0.0 
PPNPPB 1  =0.0 
PPNPPB2  =0.0 
PPNPPB3  =0.0 
PPNPPB  =0.0 

C  INITIALIZE  VRIST  ROLL  ANGLES  TO  0.0 
A6P1  =  0.0 
A6P2  =  0.0 

C  SETUP  ROTATION  PARAMETERS 
Rll  =  A(l,l) 

R21  =  A(2,1) 

R31  =  A(3,1) 

C  SETUP  UP  TRIG.  FUNCTIONS 
CIP  =  COS (VP) 

SIP  =  sin(vp) 


173 


C23PP  =  COS(APP) 

S23PP  =  SIN(APP) 

C23PN  =  COS(APN) 

S23PN  =  SIN(APN) 

C4P1  =  COS(A4Pl) 

S4P1  =  SIN(A4P1) 

C4P2  =  COS(A4P2) 

S4P2  =  SIN(A4P2) 

C5P1  =  COS(A5Pl) 

S5P1  =  SIN(A5P1) 

C5P2  =  COS(A5P2) 

S5P2  =  SIN(A5P2) 

C  COMPUTE  THE  FIRST  TERMS  FOR  THE  ARCTAN2  FUNCTION 

PPPPPAl  =  Rll  *  ((CIP  *  C23PP  *  S4P1)  -  (SIP  ♦  C4P1)) 

PPPPPA2  =  R21  *  ((SIP  ♦  C23PP  *  S4P1)  +  (CIP  ♦  C4P1)) 

PPPPPA3  =  R31  *  (S23PP  *  S4P1) 

PPPPPA  =  -  PPPPPAl  -  PPPPPA2  +  PPPPPA3 

PPNPPAl  =  Rll  *  ((CIP  ♦  C23PN  ♦  S4P2)  -  (SIP  *  C4P2)) 

PPNPPA2  =  R21  *  ((SIP  ♦  C23PN  *  S4P2)  +  (ClP  *  C4P2)) 

PPNPPA3  =  R31  *  (S23PN  *  S4P2) 

PPNPPA  =  -  PPNPPAl  -  PPNPPA2  +  PPNPPA3 
C  COMPOTE  THE  SECOND  TERMS  FOR  THE  ARCTAN2  FUNCTION 

PPPPPBl  =  Rll  *  (C5P1  ♦  ((CIP  ♦  C23PP  ♦  C4P1)  +  (SIP  ♦  S4P1)) 

$  -  (CIP  ♦  S23PP  ^  S5P1)) 

PPPPPB2  =  R21  *  (C5P1  *  ((SIP  ♦  C23PP  *  C4P1)  -  (CIP  *  S4P1)) 

$  -  (SIP  *  S23PP  ^  S5P1)) 

PPPPPB3  =  R31  ♦  ((S23PP  ♦  C4P1  ♦  C5P1)  +  (C23PP  *  S5P1)) 

PPPPPB  =  PPPPPBl  +  PPPPPB2  -  PPPPPB3 

PPNPPBl  =  Rll  *  (C5P2  *  ((CIP  ♦  C23PN  ♦  C4P2)  +  (SIP  *  S4P2)) 

$  -  (CIP  *  S23PN  ^  S5P2)) 

PPNPPB2  =  R21  *  (C5P2  ^  ((SIP  ♦  C23PN  *  C4P2)  -  (CIP  ♦  S4P2)) 

$  -  (SIP  *  S23PN  *  S5P2)) 

PPNte  =  R31  *  ((S23PN  ♦  C4P2  ♦  C5P2)  +  (C23PN  *  S5P2)) 

PPNPPB  =  PPNPPBl  +  PPNPPB2  -  PPNPPB3 
C  COMPUTE  THE  HAND  ROLL  ANGLE  USING  THE  ARCTAN2  FUNCTION 
A6P1  =  ATAN2(PPPPPA, PPPPPB) 

A6P2  =  ATAN2(PPNPPA, PPNPPB) 

RETURN 

END 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
C  RADIAN  TO  DEGREE  CONVERSION  ROUTINE 

SUBROUTINE  RADEG(RAD1,RAD2,DE61,DEG2) 

REAL  RAD1,RAD2,DEG1,DEG2,PI 
C  INITIALIZE  LOCAL  VARIABLES  AND  RETURNED  VALUES 
DEGl  =  0.0 

DEG2  =  0.0 

C  DECLARE  CONSTANTS 

PI  =  3.141592653589792 
C  PERFORM  CONVERSION 

DEGl  =  RADI  *  180.0  /  PI 

DEG2  =  RAD2  *  180.0  /  PI 

RETURN 
END 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
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C  CHECK  FOE  VALIDITY  OF  SOLUTIONS 
SUBROUTINE  VALID(A) 

REAL  A (4, 7) 

INTEGER  I,J 

C  CHECK  FOR  VALIDITY  ON  ALL  JOINTS. IF  OUT  OF  RANGE,  SET  COLUMN  1  =  1.0 

C  NOTE  THAT  THE  RANGES  ARE  OFFSET  BY  0.01  DEGREES  TO  TAKE  CARE 

C  OF  COMPUTATIONAL  ERRORS  CAUSED  BY  THE  MACHINE. 

DO  200  I  =  1,4 

C  VAIST  RANGEG  IS  +/-  147  DEGREES 

IF(ABS(A(I,2)  .GT.  147.01)  .OR. 

C  SHOULDER  RANGE  IS  +56  TO  -236  DEGREES 

$  ((A(I,3)  .GT.  56.01)  .OR.  (A(I,3)  .LT.  -236.01))  .OR. 

C  ELBOV  RANGE  IS  THE  SAME  AS  THE  SHOULDER  RANGE 

$  ((A(I,4)  .GT.  56.01)  .OR.  (A(I,4)  .LT.  -236.01))  .OR. 

C  VRIST  ROLL  IS  CONTINUOUS.  RANGE  IS  +/-  360  DEGREES 
$  ABS(A(I,5)  .GT.  360.01)  .OR. 

C  VRIST  PITCH  IS  +/-  90  DEGREES 

S  ABS(A(I,6)  .GT.  90.01)  .OR. 

C  HAND  ROLL  IS  CONTINUOUS.  RANGE  IS  +/-  360  DEGREES 
$  ABS(A(I,7)  .GT.  360.01))  THEN 

C  IF  OUT  OF  RANGE,  SET  FLAG  (COLUMN  1  OF  RESPECTIVE  ROV)  =1.0 
A(I,1)  =  1.0 
ENDIF 
200  CONTINUE 
RETURN 
END 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 

C  NORMALIZE  THE  COMPUTED  RESULTS  SO  THAT  EACH  ANGLE  RANGES 

C  FROM  -180.0  TO  180.0  DEGREES 
SUBROUTINE  NORMAL (A) 

REAL  A (4, 7) 

INTEGER  I,J 

C  NORMALIZE  THE  ANGLES  TO  BETWEEN  - 180  AND  +180  DEGREES 
DO  701  I  =  1,4 
DO  701  J  =  2,7 
IF(A(I,J)  .GT.  180.0)  THEN 
A(I,J)  =  A(I.J)  -  360.0 
ELSEIF(A(I,J)  .LT.  -180.0)  THEN 
A(I,J)  =  A(I,J)  +  360.0 
ENDIF 

701  CONTINUE 
RETURN 
END 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 


DATAFILE  ROOM.DAT 


00000000 

00000000 

00000000 
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144  144  144  144  0  0  0  0 
90  90  0  0  90  90  0  0 
108  0  108  0  108  0  108  0 

0  0  0 
0  0  0 


-72  -45  -54 
0  0  0 


DOCUMENTED  DATA  FILE  ROOM.DOC 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

WARNING: 

THIS  FILE  SERVES  AS  A  DOCUMENTATION  FOR  THE  DATA  FILE  USED  TO 
DRAW  THE  ROOM  THAT  WILL  CONTAIN  THE  MERLIN  ROBOT. 

IT  IS  NOT  TO  BE  USED  FOR  THE  ACTUAL  DRAWING. 

ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

THE  FIRST  NUMBER  TELLS  THE  HOST  PROGRAM  HOW  MANY  LINKS  COMPOSE 
THE  SYSTEM  THAT  IS  TO  BE  READ. 

2 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
EACH  LINK  IN  EACH  SYSTEM  OF  THE  CURRENT  DRAWING  ROUTINE  USED  IS  AN 
OBJECT  DEFINED  BY  EIGHT  POINTS  IN  3D  SPACE. 


THE  FIRST  ROW  OF  NUMBERS  IS  VALUES  OF  ’X’  FOR  THE  EIGHT  POINTS. 

THE  SECOND  ROW  IS  FOR  THE  ’Y’  VALUES.  THE  THIRD  ROW  IS  FOR  ’Z’  VALDES. 

THE  ORDER  OF  THE  POINTS  IS  IMPORTANT. 


/j 

- ^ 

/ 

1 

1 

s 

/ 

1 

/ 

7  : 

1 

IN  THE  PROGRAM,  THE  POINTS  ARE  CONNECTED  IN  THE  FOLLOWING  ORDER 
1,2, 4, 3, 7, 5, 6, 2, 6, 8, 4, 8, 7, 5, 1,3 
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THIS  TRACES  OUT  THE  FIGURE (WHICH  NEED  NOT  BE  A  CUBE). 

SOME  LINES  ARE  RETRACED. 

IN  DISSPLA,  THIS  IS  THE  HOST  EFFICIENT  METHOD  OF  DRAWING  THE  OBJECT. 

THE  FIRST  LINK  IN  THE  DATA  FILE  IS  BLANK,  IT  IS  AN  ORIENTATION  LINK. 
00000000 
00000000 
00000000 

THIS  IS  THE  SET  OF  POINTS  THAT  DEFINE  THE  CORNERS  OF  THE  ROOM. 

144  144  144  144  0  0  0  0 
90  90  0  0  90  90  0  0 
108  0  108  0  108  0  108  0 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
THE  FOLLOWING  NUMBERS  DEFINE  THE  TRANSLATION  VECTOR  AND  ORIENTATION 
VECTOR  OF  A  LINK  WITH  RESPECT  TO  THE  PREVIOUS  LINK. 

THEY  ARE  DEFINED  AS  X,Y,A  COORDINATE  VALDES. 

THE  FIRST  IS  BLANK,  IT  IS  AN  ORIENTATION  VECTOR. 

0  0  0 
0  0  0 

TEESE  VECTORS  PLACE  THE  BACK  LEFT  BOTTOM  CORNER  OF  THE  ROOM  WITH 
RESPECT  TO  THE  ORIGIN,  WHICH  IS  CENTER  OF  THE  SCREEN. 

-72  -45  -54  TRANSLATION  VECTOR 

000  ROTATION  VECTOR 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 


DATAFILE  MERL.DAT 

10 

ooooonoo 

00000000 

00000000 

18.75  18.75  18.75  18.75  -18.75  -18.75  -18.75  -18.75 
4.1  4.1  0  0  4.1  4.1  0  0 

18.75  -18.75  18.75  -18.75  18.75  -18.75  18.75  -18.75 

-3.12  -3.12  -3.12  -3.12  3.12  3.12  3.12  3.12 

-27.9  -27.9  0  0  -27.9  -27.9  0  0 

3.12  -3.12  3,12  -3,12  3.12  -3.12  3.12  -3.12 

-20.3  -20.3  -20.3  -20.3  7.0  7.0  7.0  7.0 
9.0  -9.0  9.0  -9.0  9.0  -9.0  9.0  -9.0 
4.9  4.9  -14.4  -14.4  4.9  4.9  -14.4  -14.4 

-15  -15  -15  -15  3  3  3  3 
-4-444-3-3  33 
3  -3  3  -3  3  -3  3  -3 
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-3  -3  -3  -3  20.3  20.3  20.3  20.3 
-3  -3  3  3  -3  -3  3  3 
3  -3  3  -3  3  -3  3  -3 

3  3  -3  -3  3  3  -3  -3 
-14-14-14-14  17.2  17.2  17.2  17.2 
3-33-33-33-3 

00000000 

00000000 

00000000 

00000000 

00000000 

00000000 

00000000 

00000000 

00000000 

0  0  0 
0  0  0 

40  0  54 
0  0  0 

0  4.1  0 
0  0  3.1416 

0  -42.35  0 
1.5708  0  0 

0  12  0 
-1.5708  0  0 

0  0  6 
0  0  0 

17.3  0  -6 
0  0  -1.5708 

0  17.2  0 
-1.5708  0  0 

0  0  0 
1.5708  0  0 

0  0  0 

-1.5708  0  0 
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DOCUIENTED  DATA  FILE  MERL.DOC 


cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

VAENIN6: 

THIS  FILE  SERVES  AS  A  DOCDMENTATION  FOR  THE  DATA  FILE  USED  TO  DRAV  THE 
MERLIN  ROBOT.  IT  IS  NOT  TO  BE  USED  FOR  THE  ACTUAL  DRAVIN6. 

DOCUMENTED  DATA  FILE:- 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

THE  FIRST  NUMBER  TELLS  THE  HOST  PROGRAM  HOV  MANY  LINKS  COMPOSE  THE 
SYSTEM  THAT  IS  TO  BE  READ. 

10 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
THE  FIRST  LINK  IN  THE  DATA  FILE  MUST  BE  A  BLANK  LINK  IF  THE  SYSTEM  IS  TO 
BE  APPENDED  TO  ANOTHER  SYSTEM  (THE  ROBOT  IS  GOING  TO  BE  PLACED  IN  THE 
ROOM,  VHICH  IS  ITS  OVN  SYSTEM). 

00000000 

00000000 

00000000 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
EACH  LINK  IN  EACH  SYSTEM  OF  THE  CURRENT  DRAWING  ROUTINE  USED  IS  AN 
OBJECT  DEFINED  BY  EIGHT  POINTS  IN  3D  SPACE. 

THE  FIRST  ROW  OF  NUMBERS  IS  VALUES  OF  ’X’  FOR  THE  EIGHT  POINTS. 

THE  SECOND  ROW  IS  FOR  THE  ’Y’  VALUES.  THE  THIRD  ROW  IS  FOR  ’Z’  VALUES. 

THE  ORDER  OF  THE  POINTS  IS  IMPORTANT. 


/ 1 

/ 

1 

/ 

1 

1 

1 

fi 

/ 

/ 

r 

/ 

IN  THE  PROGRAM,  THE  POINTS  ARE  CONNECTED  IN  THE  FOLLOWING  ORDER 
1,2, 4, 3, 7, 5, 6, 2, 6, 8, 4, 8, 7, 5, 1,3 

THIS  TRACES  OUT  THE  FIGURE(WHICH  NEED  NOT  BE  A  CUBE).  SOME  LINES  ARE 
RETRACED.  IN  DISSPLA,  THIS  IS  THE  MOST  EFFICIENT  METHOD  OF  DRAWING  THE 
OBJECT. 

THIS  IS  THE  SET  OF  POINTS  REPRESENTING  THE  BASE  SUPPORT. 

18.75  18.75  18.75  18.75  -18.75  -18.75  -18.75  -18.75 
4.1  4.1  0  0  4.1  4.1  0  0 

18.75  -18.75  18.75  -18.75  18.75  -18.75  18.75  -18.75 
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THIS  IS  THE  SET  OF  POINTS  REPRESENTING  THE  COLUMN  BETWEEN  THE  BASE  k  THE 
MOTOR  HOUSING. 

-3.12  -3.12  -3.12  -3.12  3.12  3.12  3.12  3.12 

-27.9  -27.9  0  0  -27.9  -27.9  0  0 

3.12  -3.12  3.12  -3.12  3.12  -3.12  3.12  -3.12 

THIS  IS  THE  SET  OF  POINTS  REPRESENTING  THE  VAIST  AND  MOTOR  HOUSING. 

-20.3  -20.3  -20.3  -20.3  7.0  7.0  7.0  7.0 
9.0  -9.0  9.0  -9.0  9.0  -9.0  9.0  -9.0 
4.9  4.9  -14.4  -14.4  4.9  4.9  -14.4  -14.4 

THIS  IS  THE  SET  OF  POINTS  REPRESENTING  THE  COUNTERWEIGHT  OF  SHOULDER. 

-15  -15  -15  -15  3  3  3  3 
-4  -4  4  4  -3  -3  3  3 
3  -3  3  -3  3  -3  3  -3 

THIS  IS  THE  SET  OF  POINTS  REPRESENTING  THE  UPPER  ARM. 

-3  -3  -3  -3  20.3  20.3  20.3  20.3 
-3  -3  3  3  -3  -3  3  3 
3  -3  3  -3  3  -3  3  -3 

THIS  IS  THE  SET  OF  POINTS  REPRESENTING  THE  LOWER  ARM. 

THE  COUNTERWEIGHT  IS  INCLUDED  IN  THIS  DATA  SET. 

3  3  -3  -3  3  3  -3  -3 

-14  -14  -14  -14  17.2  17.2  17.2  17.2 

3  -3  3  -3  3  -3  3  -3 

THIS  IS  THE  SET  OF  POINTS  REPRESENTING  THE  WRIST  ROLL.  IT  IS  BLANK 

BECAUSE  IT  HAS  NO  PHYSICAL  DIMENSIONS,  BUT  BECAUSE  OF  THE  WAY  THE 

PROGRAM  IS  SET  UP,  THE  LINK  MUST  HAVE  DIMENSIONS  EVEN  IF  THEY  ARE  ZERO. 

00000000 

00000000 

00000000 

THIS  IS  THE  SET  OF  POINTS  REPRESENTING  THE  WRIST  PITCH. 

00000000 

00000000 

00000000 

THIS  IS  THE  SET  OF  POINTS  REPRESENTING  THE  HAND  ROLL. 

00000000 

00000000 

00000000 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
THE  FOLLOWING  NUMBERS  DEFINE  THE  TRANSLATION  VECTOR  AND  ROTATION  VECTOR 
OF  A  LINK  WITH  RESPECT  TO  THE  PREVIOUS  LINK. 

THE  FIRST  SET  MUST  BE  BLANK  SO  THAT  THE  MERLIN  CAN  BE  APPENDED  TO  THE 
ROOM. 

THE  ZEROES  WILL  BE  REPLACED  IN  THE  PROGRAM  WITH  THE  VECTORS  OF  THE  LAST 
LINK  OF  THE  SYSTEM  TO  BE  APPENDED  TO. 
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000-  TRANSLATION  VECTOR 
000-  ROTATION  VECTOR 


THIS  IS  THE  TRANSLATION  VECTOR  OF  THE  BASE. 

THEY  ARE  DEFINED  AS  X,Y,Z  COORDINATES  OF  THE  BOTTOM  CENTER  OF  THE  BASE 
WITH  RESPECT  TO  THE  FAR  LEFT  LOVER  CORNER  OF  THE  ROOM. 

IN  THE  PROGRAM,  WHEN  THE  ROBOT  IS  TO  BE  REPOSITIONED,  THESE  NDMBERS  WILL 
BE  CHANGED  BY  THE  USER.  THE  DEFAULT  IS  SEf  TO  [54,0,54] 

THESE  ARE  THE  VECTORS  OF  THE  ROBOT’S  BASE  VITH  RESPECT  TO  THE  ROOM. 

54  0  54  -  TRANSLATION  VECTOR 

000  -  ROTATION  VECTOR 

THESE  ARE  THE  VECTORS  OF  THE  ROBOT’S  COLUMN  VITH  RESPECT  TO  THE  BASE. 

0  4.1  0  -  TRANSLATION  VECTOR 

0  0  3.1416  -  ROTATION  VECTOR 

THESE  ARE  THE  VECTORS  OF  THE  ROBOT’S  VAIST  VITH  RESPECT  TO  THE  COLUMN. 

0  -42.35  0  -  TRANSLATION  VECTOR 

1.5708  0  0  -  ROTATION  VECTOR 

THESE  ARE  THE  VECTORS  OF  THE  ROBOT’S  COUNTERVEIGHT  VITH  RESPECT  TO  THE 
VAIST. 

0  12  0  -  TRANSLATION  VECTOR 

-1.5708  0  0  -  ROTATION  VECTOR 

THESE  ARE  THE  VECTORS  OF  THE  ROBOT’S  UPPER  ARM  VITH  RESPECT  TO  THE 
SHOULDER  COUNTERVEIGHT. 

006  -  TRANSLATION  VECTOR 

000  -  ROTATION  VECTOR 

THESE  ARE  THE  VECTORS  OF  THE  ROBOT’S  LOVER  ARM  VITH  RESPECT  TO  THE  UPPER 

ARM 

17.3  0  -6  -  TRANSLATION  VECTOR 

00-1  708  -  ROTATION  VECTOR 

THESE  ARE  THE  VECTORS  OF  THE  ROBOT’S  VRIST  ROLL  VITH  RESPECT  TO  THE 
LOVER  ARM 

0  17.2  0  ’  -  TRANSLATION  VECTOR 

-1.5708  0  0  ROTATION  VECTOR 

THESE  ARE  THE  VECTORS  OF  THE  ROBOT’S  VRIST  PITCH  VITH  RESPECT  TO  THE 
VRIST  ROLL. 

000  -  TRANSLATION  VECTOR 

1.5708  0  0  -  ROTATION  VECTOR 

THESE  ARE  THE  VECTORS  OF  THE  ROBOT’S  HAND  RUI;  Viil  RESPECT  TO  THE  VRIST 
PITCH. 

000  -  TRANSLATION  VECTOR 

-1.5708  0  0  -  ROTATION  VECTOR 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCi.CCCCCCCCrCCCCCCCCCCCCCC 
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DATAFILE  DTAH.DAT 


4 


00000000 

00000000 

00000000 

00000000 

00000000 

00000000 

00000000 

00000000 

00000000 

1.8  -1.9  1.8  -1.9  1.8  -1.9  1.8  -1.9 

4.25  4.25  4.25  4.25  0000 

.375  .375  -1.5  -1.5  .375  .375  -.375  -.375 

0  0  0 
0  0  0 

0  0  0 
0  0  0 

0  0  0 

-1.5708  0  3.1416 
0  0  0 

0  1.5708  0 
5 

00000000 

00000000 

00000000 

.5  -  .5  .5  .5  -  .5  -  .5  .5  .5 
-.5  .5  -  .5  .5  -  .5  .5  -  .5  .5 
1.2  1.2  1.2  1.2  0  0  0  0 

1.7  1.7  1.7  1.7  0  0  0  0 

.45  .45  -  .45  - .45  .45  .45  - .45  - .45 

.45  -.45  .45  -.45  .45  -.45  .45  -.45 

1.3  1.3  1.3  1.3  0  0  0  0 
.4  .4  - .4  - .4  .4  .4  -  .4  -  .4 
.4  -  .4  .4  -  .4  .4  -  .4  .4  -  .4 
1.0625  1.0625  1.0625  1.0625  0000 
.35  .35  -  .35  - .35  .35  .35  - .35  - .35 
.35  -  .35  .35  - .35  .35  - .35  .35  - .35 
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0  0  0 
0  0  0 

-1.375  4.25  -  .75 
-1.34  0  1.5708 

0  0  1.2 

1.5708  0  1.5708 

1.7  0  0 
0  0  0 

1.3  0  0 
0  0  0 

5 

00000000 

00000000 

00000000 

-  -5  - .5  .5  .5  - .5  - .5  .5  .5 

- .5  .5  - .5  .5  - .5  .5  - .5  .5 

1.2  1.2  1.2  1.2  0  0  0  0 

1.7  1.7  1.7  1.7  0  0  0  0 

.45  .45  - .45  - .45  .45  .45  - .45  -  .45 

.45  - .45  .45  - .45  .45  -  .45  .45  -  .45 

1.3  1.3  1.3  1.3  0  0  0  0 
.4  .4  - .4  - .4  .4  .4  - .4  - .4 
.4  -  .4  .4  -  .4  .4  - .4  .4  - .4 

1.0625  1.0625  1.0625  1.0625  0000 
.35  .35  - .35  - .35  .35  .35  .35  .35 

.35  - .35  .35  .35  .35  - .35  .35  .35 

0  0  0 
0  0  0 

.10  4.25  - .75 
1.34  0  1.5708 

0  0  1.2 

l.'^TOS  0  1.5708 

1.7  0  0 
0  0  0 

1.3  0  0 
0  0  0 

5 


00000000 

00000000 

00000000 


-  .5  - .5  .5  .5  - .5  - .5  .5  .5 
-.5  .5  -.5  .5  -.5  .5  -.5  .5 
1.2  1.2  1.2  1.2  0  0  0  0 

1.7  1.7  1.7  1.7  0  0  0  0 

.45  .45  -.45  -.45  .45  .45  -.45  -.45 

.45  -.45  .45  -.45  .45  -.45  .45  -.45 

1.3  1.3  1.3  1.3  0  0  0  0 

.4  .4  -  .4  -  .4  .4  .4  -  .4  -  .4 
.4  -.4  .4  -.4  .4  -.4  .4  -.4 

1.0625  1.0625  1.0625  1.0625  0000 
.35  .35  -.35  -.35  .35  .35  -.35  -.35 
.35  -.35  .35  -.35  .35  -.35  .35  -.35 

0  0  0 
0  0  0 

1.1875  4.25  - .75 
-1.34  0  1.5708 

0  0  1.2 

1.5708  0  1.5708 

1.7  0  0 
0  0  0 

1.3  0  0 
0  0  0 


00000000 

00000000 

00000000 

00000000 

00000000 

00000000 

1.7  1.7  0  0  1.7  1.7  0  0 
.5  .5  .5  .5  - .5  - .5  - .5  -  .5 
—  .5  .5  — .5  .5  — .5  .5  — .5  .5 

1.3125  1.3125  0  0  1.3125  1.3125  0  0 
.45  .45  .45  .45  -.45  -.45  -.45  -.45 
-.45  .45  -.45  .45  -.45  .45  -.45  .45 
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1.125  1.125  0  0  1.125  1.125  0  0 
.4  .4  .4  .4  -  .4  -  .4  -  .4  -  .4 
-  .4  .4  -  .4  .4  -  .4  .4  -  .4  .4 

0  0  0 
0  0  0 

.75  3.125  0 
-1.5708  0  -1.5708 

.375  0  0 
1.5708  0  0 

1.7  0  0 
0  0  0 

1.3125  0  0 
0  0  0 


.375  .375  - .375  - .375  .375  .375  - .375  - .375 
3.875  3.875  3.875  3.875  0000 
-1.9  1.8  -1.9  1.8  -1.9  1.8  -1.9  1.8 


DOCUMENTED  DATA  FILE  UTAH.DOC 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

WARNING : 

THIS  FILE  SERVES  AS  A  DOCUMENTATION  FOR  THE  DATA  FILE  USED  TO  DRAW 
THE  UTAH  HAND.  IT  IS  NOT  TO  BE  USED  FOR  THE  ACTUAL  DRAWING. 

DOCUMENTED  DATA  FILE:- 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
THE  FIRST  NUMBER  TELLS  THE  HOST  PROGRAM  HOW  MANY  LINKS  COMPOSE 
THE  SYSTEM  THAT  IS  TO  BE  READ. 

4 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
THE  FIRST  LINK  IN  THE  DATA  FILE  OF  SYSTEM  MUST  BE  A  BLANK  LINK  IF  THE 
SYSTEM  IS  TO  BE  APPENDED  TO  ANOTHER  SYSTEM  (THE  HAND  IS  GOING  TO  BE 
PLACED  ON  THE  MERLIN  ROBOT) . 

00000000 

00000000 

00000000 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
THE  SECOND  LINK  IN  THE  DATA  FILE  FOR  THE  HAND  MUST  BE  A  BLANK. 

IN  THE  PROGRAM,  THE  USER  HAS  THE  OPTION  OF  REPOSITIONING  THE  HAND. 
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TO  lAKE  THIS  POSSIBLE,  A  BLANK  LINK  IS  REQUIKED. 


00000000 

00000000 

00000000 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 
THE  THIRD  LINK  IN  THE  DATA  FILE  FOR  THE  HAND  lUST  BE  A  BLANK.  THIS  IS 
NECESSARY  TO  ACHIEVE  PROPER  ORIENTATION  OF  THE  HAND. 

00000000 

00000000 

00000000 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 
EACH  LINK  IN  EACH  SYSTEH  OF  THE  CURRENT  DRAWING  ROUTINE  USED  IS  AN 
OBJECT  DEFINED  BY  EIGHT  POINTS  IN  3D  SPACE. 

THE  FIRST  ROV  OF  NUMBERS  IS  VALUES  OF  ’I*  FOR  THE  EIGHT  POINTS. 

THE  SECOND  ROV  IS  FOR  THE  ’Y’  VALDES.  THE  THIRD  ROV  IS  FOR  ’Z’  VALUES. 

THE  ORDER  OF  THE  POINTS  IS  IMPORTANT. 


IN  THE  PROGRAM,  THE  POINTS  ARE  CONNECTED  IN  THE  FOLLOWING  ORDER 
1,2, 4, 3, 7, 5, 6, 2, 6, 8, 4, 8, 7, 5, 1,3 

THIS  TRACES  OUT  THE  FIGURE(VHICH  NEED  NOT  BE  A  CUBE).  SOME  LINES  ARE 
RETRACED.  IN  DISSPLA,  THIS  IS  THE  HOST  EFFICIENT  METHOD  OF  DRAWING  TEE 
OBJECT. 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
THIS  IS  THE  SET  OF  POINTS  REPRESENTING  THE  PALM  SECTION. 

1.8  -1.9  1.8  -1.9  1.8  -1.9  1.8  -1.9 

4.25  4.25  4.25  4.25  0000 

.375  .375  -1.5  -1.5  .375  .375  -.375  -.375 


CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
THE  FOLLOWING  NUMBERS  DEFINE  THE  TUNSLATION  VECTOR  AND  ROTATION  VECTOR 
OF  THAT  LINK  WITH  RESPECT  TO  THE  PREVIOUS  LINK.  THEY  ARE  DEFINED  AS 
X,Y,Z  COORDINATE  VALDES. 

THE  FIRST  MUST  BE  BLANK  IF  IT  IS  TO  APPENDED  TO  ANOTHER  SYSTEM 

THE  ZEROES  WILL  BE  REPLACED  IN  THE  PROGRAM  WITH  THE  VECTORS  OF  THE  LAST 
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LINK  OF  THE  SYSTEl  TO  BE  APPENDED  TO. 

000  THANSLATION  VECTOR 

000  ROTATION  VECTOR 

THE  SECOND  HOST  BE  BIANE  TO  ALLOY  FOR  REPOSITION  OF  THE  HAND. 

THE  ZEROES  BILL  BE  REPLACED  IN  THE  PR06RA1  VITH  THE  VECTORS  THAT  DEFINE 
THE  REPOSITIONING  OF  THE  HAND. 

000  TRANSLATION  VECTOR 

000  ROTATION  VECTOR 


THESE  ARE  THE  TRANSLATION  AND  ROTATION  VECTORS  OF  SECOND  AND  THIRD  EHPTY 
LINKS. 

000  TRANSLATION  VECTOR 

- 1 . 57  0  3 . 14  ROTATION  VECTOR 

THESE  ARE  VECTORS  OF  THE  PALI  VITH  RESPECT  TO  TEE  THIRD  EHPTY  LINK. 

000  TRANSLATION  VECTOR 

0  1.5708  0  ROTATION  VECTOR 


CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
NOHBER  OF  LINKS  OF  THE  FIRST  FINGER. 

5 

EMPTY  LINK  FOR  APPENDING  THE  FIRST  FINGER  TO  THE  PALM. 
00000000 
00000000 
00000000 

THIS  IS  THE  SET  OF  POINTS  REPRESENTING  THE  (ZERO)TH  LINK  SECTION. 

-  .5  - .5  .5  .5  - .5  - .5  .5  .5 

-  .5  .5  - .5  .5  - .5  .5  - .5  .5 

1.2  1.2  1.2  1.2  0  0  0  0 

THIS  IS  THE  SET  OF  POINTS  REPRESENTING  THE  FIRST  LINK  SECTION. 

1.7  1.7  1.7  1.7  0  0  0  0 

.45  .45  -.45  -.45  .45  .45  -.45  -.45 

.45  -.45  .45  -.45  .45  -.45  .45  -.45 


THIS  IS  THE  SET  OF  POINTS  REPRESENTING  THE  SECOND  LINK  SECTION. 

1.3  1.3  1.3  1.3  0  0  0  0 
.4  .4  -  .4  -  .4  .4  .4  -  .4  -  .4 
.4  -.4  .4  -.4  .4  -.4  .4  -.4 

THIS  IS  THE  SET  OF  POINTS  REPRESENTING  THE  THIRD  LINK  SECTION. 

1.0625  1.0625  1.0625  1.0625  0000 
.35  .35  -.35  -.35  .35  .35  -.35  -.35 
.35  -.35  .35  .35  .35  -.35  .35  -.35 

EMPTY  TRANSLATION  AND  ROTATION  VECTORS  FOR  APPENDING  THE  FINGER  TO  THE 
HAND. 

0  0  0 
0  0  0 
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THESE  iHE  THE  VECTORS  OF  THE  ZEROTH  LINK  VITH  RESPECT  TO  THE  HAND. 

- 1 . 375  4 . 25  - . 75  TRANSLATION  VECTOR 

-1.34  0  1.5708  ROTATION  VECTOR 

THESE  ARE  THE  VECTORS  OF  THE  FIRST  LINK  VITH  RESPECT  TO  THE  (ZERO)TH. 
0  0  1.2  TRANSLATION  VECTOR 

1.5708  0  1.5708  ROTATION  VECTOR 

THESE  ARE  THE  VECTORS  OF  THE  SECOND  LINK  VITH  RESPECT  TO  THE  FIRST. 
1.7  0  0  TRANSLATION  VECTOR 

000  ROTATION  VECTOR 

THESE  ARE  THE  VECTORS  OF  THE  THIRD  LINK  VITH  RESPECT  TO  THE  SECOND. 
1.3  0  0  TRANSLATION  VECTOR 

000  ROTATION  VECTOR 


CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
NUMBER  OF  LINKS  OF  THE  SECOND  FINGER. 

5 

EMPTY  LINK  FOR  APPENDING  THE  SECOND  FINGER  TO  THE  PALM. 
00000000 
00000000 
00000000 

THIS  IS  THE  SET  OF  POINTS  REPRESENTING  THE  (ZERO)TH  LINK  SECTION. 

-.5  -.5  .5  .5  -.5  -.5  .5  .5 
-.5  .5  -.5  .5  -.5  .5  -.5  .5 
1.2  1.2  1.2  1.2  0  0  0  0 

THIS  IS  THE  SET  OP  POINTS  REPRESENTING  THE  FIRST  LINK  SECTION. 

1.7  1.7  1.7  1.7  0  0  0  0 

.45  .45  -.45  -.45  .45  .45  -.45  -.45 

.45  -.45  .45  -.45  .45  -.45  .45  -.45 

THIS  IS  THE  SET  OF  POINTS  REPRESENTING  THE  SECOND  LINK  SECTION. 

1.3  1.3  1.3  1.3  0  0  0  0 
.4  .4  -  .4  -  .4  .4  .4  -  .4  -  .4 
.4  - .4  .4  - .4  .4  - .4  .4  - .4 

THIS  IS  THE  SET  OF  POINTS  REPRESENTING  THE  THIRD  LINK  SECTION. 

1.0625  1.0625  1.0625  1.0625  0000 
.35  .35  -.35  -.35  .35  .35  -.35  -.35 
.35  -.35  .35  -.35  .35  -.35  .35  -.35 


EMPTY  TRANSLATION  AND  ROTATION  VECTORS  FOR  APPENDING  THE  FINGER  TO  THE 
HAND. 

0  0  0 
0  0  0 

THESE  ARE  THE  VECTORS  OF  THE  (ZERO)TH  LINK  VITH  RESPECT  TO  THE  HAND. 

- .10  4.25  - .75  TRANSLATION  VECTOR 

-1.34  0  1.5708  ROTATION  VECTOR 
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THESE  UE  THE  VECTOIS  OF  THE  FIRST  LINK  VITH  RESPECT  TO  THE  (ZERO)TH. 
0  0  1.2  TRANSLATION  VECTOR 

1.5708  0  1.5708  ROTATION  VECTOR 

THESE  ARE  THE  VECTORS  OF  THE  SECOND  LINK  VITH  RESPECT  TO  TEE  FIRST. 
1.7  0  0  TUNSLATION  VECTOR 

000  ROTATION  VECTOR 

THESE  ARE  THE  VECTORS  OF  THE  THIRD  LINK  VITH  RESPECT  TO  THE  SECOND. 
1.3  0  0  TRANSLATION  VECTOR 

000  ROTATION  VECTOR 


CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
NDHBER  OF  LINKS  OF  TIE  THIRD  FINGER. 

5 

EHPTY  LINK  FOR  APPENDING  THE  THIRD  FINGER  TO  THE  PALI. 
00000000 
00000000 
00000000 

THIS  IS  THE  SET  OF  POINTS  REPRESENTING  THE  (ZERO)TH  LINK  SECTION. 

-.5  .5  -  .5  .5  -  .5  .5  -  .5  .5 

1.2  1.2  1.2  1.2  0  0  0  0 

THIS  IS  THE  SET  OF  POINTS  REPRESENTING  THE  FIRST  LINK  SECTION. 

1.7  1.7  1.7  1.7  0  0  0  0 

.45  .45  -.45  -.45  .45  .45  -.45  -.45 

.45  -.45  .45  -.45  .45  -.45  .45  -.45 

THIS  IS  THE  SET  OF  POINTS  REPRESENTING  THE  SECOND  LINK  SECTION. 

1.3  1.3  1.3  1.3  0  0  0  0 
.4  .4  - .4  - .4  .4  .4  - .4  - .4 
.4  -.4  .4  -.4  .4  -.4  .4  -.4 

THIS  IS  THE  SET  OF  POINTS  REPRESENTING  TEE  THIRD  LINK  SECTION. 

1.0625  1.0625  1.0625  1.0625  0000 
.35  .35  -.35  -.35  .35  .35  -.35  -.35 

.35  -.35  .35  -.35  .35  -.35  .35  -.35 


EHPTY  TRANSLATION  AND  ROTATION  VECTORS  FOR  APPENDING  THE  FINGER  TO  THE 
HAND. 

0  0  0 
0  0  0 

THESE  ARE  THE  VECTORS  OP  THE  (ZERO)TH  LINK  VITH  RESPECT  TO  THE  HAND. 
1.1875  4.25  -.75  TRANSLATION  VECTOR 

-1.34  0  1.5708  ROTATION  VECTOR 

THESE  ARE  THE  VECTORS  OF  THE  FIRST  LINK  VITH  RESPECT  TO  THE  (ZERO)TH. 

0  0  1.2  TUNSLATION  VECTOR 

1.5708  0  1.5708  ROTATION  VECTOR 
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THESE  ARE  TEE  VECTORS  OF  THE  SECOND  LINK  VITH  RESPECT  TO  THE  FIRST. 
1.7  0  0  TRANSLATION  VECTOR 

000  ROTATION  VECTOR 

THESE  ARE  THE  VECTORS  OF  THE  THIRD  LINK  VITH  RESPECT  TO  THE  SECOND. 
1.3  0  0  TRANSLATION  VECTOR 

000  ROTATION  VECTOR 


cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

NUMBER  OF  LINKS  OF  THE  THUMB. 

5 

EMPTY  LINK  FOR  APPENDING  THE  THUMB  TO  THE  PALM. 

00000000 

00000000 

00000000 

THIS  IS  THE  SET  OF  POINTS  REPRESENTING  THE  (ZERO)TH  LINK  SECTION. 
00000000 
00000000 
00000000 

THIS  IS  THE  SET  OF  POINTS  REPRESENTING  THE  FIRST  LINK  SECTION. 

1.7  1.7  0  0  1.7  1.7  0  0 
.5  .5  .5  .5  -.5  -.5  -.5  -.5 


THIS  IS  THE  SET  OF  POINTS  REPRESENTING  THE  SECOND  LINK  SECTION. 
1.3125  1.3125  0  0  1.3125  1.3125  0  0 
.45  .45  .45  .45  -.45  -.45  -.45  -.45 
-.45  .45  -.45  .45  -.45  .45  -.45  .45 


THIS  IS  THE  SET  OF  POINTS  REPRESENTING  THE  THIRD  LINK  SECTION. 

1.125  1.125  0  0  1.125  1.125  0  0 
.4  .4  .4  .4  -.4  -.4  -.4  -.4 
-  .4  .4  -  .4  .4  - .4  .4  - ,4  .4 

EMPTY  TRANSLATION  AND  ROTATION  VECTORS  FOR  APPENDING  THE  THUMB  TO  THE 
HAND. 

0  0  0 
0  0  0 

THESE  ARE  THE  VECTORS  OF  THE  (ZERO)TH  LINK  VITH  RESPECT  TO  THE  HAND. 

.75  3.125  0  TRANSLATION  VECTOR 

- 1 . 5708  0  - 1 . 5708  ROTATION  VECTOR 

THESE  ARE  THE  VECTORS  OF  THE  FIRST  LINK  VITH  RESPECT  TO  THE  (ZERO)TH 
LINK. 

.375  0  0  TRANSLATION  VECTOR 
1.5708  0  0  ROTATION  VECTOR 
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TIESE  lEE  TIE  VECTORS  OF  THE  SECOND  LINK  VITE  RESPECT  TO  TIE  FIRST. 
1.7  0  0  TRANSLATION  VECTOR 

000  ROTATION  VECTOR 


THESE  ARE  THE  VECTORS  OF  THE  THIRD  LINK  VITH  RESPECT  TO  THE  SECOND. 
1.3125  0  0  TUNSLATION  VECTOR 
0  0  0  ROTATION  VECTOR 
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